/* Copyright (c) 2017, Waterloo Autonomous Vehicles Laboratory (WAVELab),
 * Waterloo Intelligent Systems Engineering Lab (WISELab),
 * University of Waterloo.
 *
 * Refer to the accompanying LICENSE file for license information.
 *
 * ############################################################################
 ******************************************************************************
 |                                                                            |
 |                         /\/\__/\_/\      /\_/\__/\/\                       |
 |                         \          \____/          /                       |
 |                          '----________________----'                        |
 |                              /                \                            |
 |                            O/_____/_______/____\O                          |
 |                            /____________________\                          |
 |                           /    (#UNIVERSITY#)    \                         |
 |                           |[**](#OFWATERLOO#)[**]|                         |
 |                           \______________________/                         |
 |                            |_""__|_,----,_|__""_|                          |
 |                            ! !                ! !                          |
 |                            '-'                '-'                          |
 |       __    _   _  _____  ___  __  _  ___  _    _  ___  ___   ____  ____   |
 |      /  \  | | | ||_   _|/ _ \|  \| |/ _ \| \  / |/ _ \/ _ \ /     |       |
 |     / /\ \ | |_| |  | |  ||_||| |\  |||_|||  \/  |||_||||_|| \===\ |====   |
 |    /_/  \_\|_____|  |_|  \___/|_| \_|\___/|_|\/|_|\___/\___/ ____/ |____   |
 |                                                                            |
 ******************************************************************************
 * ############################################################################
 *
 * File: pose_cov_comp.cpp
 * Desc: File containing the impl. for pose composition with uncertainty
 * Auth: Chunshang Li and Jordan Hu
 *
 * ############################################################################
 */

#include "wave/utils/pose_cov_comp.hpp"
#include <Eigen/Dense>

namespace wave {

PoseWithCovariance::PoseWithCovariance() {
    this->position.setZero();
    this->rotation_matrix.setIdentity();
    this->covariance.setZero();
}

PoseWithCovariance::PoseWithCovariance(Vector6 &p, Matrix6x6 &cov) {
    Vector3 ypr;

    ypr << p[5], p[4], p[3];

    this->position = p.block<3, 1>(0, 0);
    this->rotation_matrix = pose_comp::yprToRotMatrix(ypr);
    this->covariance = cov;
}

PoseWithCovariance::PoseWithCovariance(Vector3 &p,
                                       Matrix3x3 &r,
                                       Matrix6x6 &cov) {
    this->position = p;
    this->rotation_matrix = r;
    this->covariance = cov;
}

Vector3 PoseWithCovariance::getPosition() const {
    return this->position;
}

Vector3 PoseWithCovariance::getYPR() const {
    Vector3 ypr = pose_comp::rotMatrixToYPR(this->rotation_matrix);
    return ypr;
}

Eigen::Quaterniond PoseWithCovariance::getQuaternion() const {
    Eigen::Quaterniond q = pose_comp::rotMatrixToQuat(this->rotation_matrix);

    q.normalize();

    return q;
}

Vector7 PoseWithCovariance::getPoseQuaternion() const {
    Vector7 pose;
    Eigen::Quaterniond q = this->getQuaternion();
    Vector4 q_coeffs;

    q_coeffs << q.coeffs().w(), q.coeffs().x(), q.coeffs().y(), q.coeffs().z();
    pose << this->position, q_coeffs;

    return pose;
}

Eigen::Affine3d PoseWithCovariance::getTransformMatrix() const {
    Eigen::Affine3d T_m;

    T_m.translation() = this->position;
    T_m.linear() = this->rotation_matrix;

    return T_m;
}

PoseWithCovariance composePose(PoseWithCovariance &p1, PoseWithCovariance &p2) {
    PoseWithCovariance r;  // store all the results

    // Use Eigen Transform to transform poses
    // This is the same as implementing Equation (5.5)
    Eigen::Affine3d T_p1, T_p2, T_r;
    T_p1 = p1.getTransformMatrix();
    T_p2 = p2.getTransformMatrix();
    T_r = T_p1 * T_p2;

    r.position = T_r.translation();
    r.rotation_matrix = T_r.rotation();

    // compute the covariances
    // p6 = [x y z roll pitch yaw]', transformation using YPR
    // p7 = [x y z qr, qx, qy, qz]', transformation using Quaternion, qr is qw

    // get covariances from PoseWithCovariance objects
    Matrix6x6 cov_p1 = p1.covariance, cov_p2 = p2.covariance;

    // pR7 is the composed pose in p7 form
    // p17 is the p1 in p7 form, p27 is p2 in p7 form, p16 is p1 in p6 form,
    // p26 is p2 in p6 form
    Vector7 pR7, p17, p27;
    Vector6 p16, p26;

    pR7 = r.getPoseQuaternion();
    p17 = p1.getPoseQuaternion();
    p27 = p2.getPoseQuaternion();

    p16 << p1.getPosition(), pose_comp::quatToYPR(p1.getQuaternion());
    p26 << p2.getPosition(), pose_comp::quatToYPR(p2.getQuaternion());

    // Equation (5.3)
    Matrix6x7 jacobian_p7_to_p6 = jacobian_p7_to_p6_wrt_p(pR7);
    Matrix7x7 jacobian_p7_p7_composition =
      jacobian_p7_p7_Composition_wrt_p1(p17, p27);
    Matrix7x6 jacobian_p6_to_p7 = jacobian_p6_to_p7_wrt_p(p16);
    Matrix6x6 dfpc_dp =
      jacobian_p7_to_p6 * jacobian_p7_p7_composition * jacobian_p6_to_p7;

    // Equation (5.4)
    jacobian_p7_p7_composition = jacobian_p7_p7_Composition_wrt_p2(p17, p27);
    jacobian_p6_to_p7 = jacobian_p6_to_p7_wrt_p(p26);
    Matrix6x6 dfpc_dq =
      jacobian_p7_to_p6 * jacobian_p7_p7_composition * jacobian_p6_to_p7;

    // Equation (5.2)
    // putting everything together
    Matrix6x6 final_cov = dfpc_dp * cov_p1 * dfpc_dp.transpose() +
                          dfpc_dq * cov_p2 * dfpc_dq.transpose();

    r.covariance = final_cov;

    return r;
}

/// the jacobian of quaternion normalization function
/// quat in the form of [qr, qx, qt, qz]
/// Equation (1.7)
Matrix4x4 jacobian_Quat_Norm_wrt_q(const Vector4 &q) {
    const double &qr = q(0), &qx = q(1), &qy = q(2), &qz = q(3);
    Matrix4x4 m;

    // See Equation (1.7)
    double k = 1 / pow(qr * qr + qx * qx + qy * qy + qz * qz, 1.5);

    // clang-format off
    m << qx * qx + qy * qy + qz * qz, -qr * qx, -qr * qy, -qr * qz,
        -qx * qr, qr * qr + qy * qy + qz * qz, -qx * qy, -qx * qz,
        -qy * qr, -qy * qx, qr * qr + qx * qx + qz * qz, -qy * qz,
        -qz * qr, -qz * qx, -qz * qy, qr * qr + qx * qx + qy * qy;
    // clang-format on

    m = m * k;

    return m;
}

// the jacobian of normalized quaternion to rpy function
// Equation (2.9) to Equation (2.10)
Matrix3x4 jacobian_Quat_Norm_to_Rpy_wrt_q(const Vector4 &q) {
    const double &qr = q(0), &qx = q(1), &qy = q(2), &qz = q(3);

    Matrix3x4 m;

    // Equation (2.9)
    double delta = qr * qy - qx * qz;

    // handle special (rare) cases for when |delta| = 0.5
    if (fabs(delta - 0.5) < 1e-10) {  // delta = 0.5
                                      // clang-format off
        m << (2*qx)/(qr*qr + qx*qx), -(2*qr)/(qr*qr + qx*qx), 0, 0,
                                    0,                     0, 0, 0,
                                    0,                     0, 0, 0;

        // clang-format on

    } else if (fabs(delta + 0.5) < 1e-10) {  // delta = -0.5
                                             // clang-format off
        m << -(2*qx)/(qr*qr + qx*qx), (2*qr)/(qr*qr + qx*qx), 0, 0,
                                     0,                    0, 0, 0,
                                     0,                    0, 0, 0;
                                             // clang-format on
    } else {
        // Equation (2.10)
        // Jacobian obtained symbolically from SymPy by taking the derivative
        // of this below
        //     roll  = [[        2*(qr*qz + qx*qy)/(-2*qy*qy - 2*qz*qz + 1)],
        //     pitch =  [                           asin(2*qr*qy - 2*qx*qz)],
        //     yaw   =  [atan((2*qr*qx + 2*qy*qz)/(-2*qx*qx - 2*qy*qy + 1))]]
        // with respect to qr, qx, qy, qz using SymPy

        // just for convenience
        auto sq = [](double x) { return x * x; };

        // clang-format off
        m <<
            2*qz*(-2*qy*qy - 2*qz*qz + 1)/(sq(2*qr*qz + 2*qx*qy) + sq(-2*qy*qy - 2*qz*qz + 1)),
            2*qy*(-2*qy*qy - 2*qz*qz + 1)/(sq(2*qr*qz + 2*qx*qy) + sq(-2*qy*qy - 2*qz*qz + 1)),
            2*qx*(-2*qy*qy - 2*qz*qz + 1)/(sq(2*qr*qz + 2*qx*qy) + sq(-2*qy*qy - 2*qz*qz + 1)) - 4*qy*(-2*qr*qz - 2*qx*qy)/(sq(2*qr*qz + 2*qx*qy) + sq(-2*qy*qy - 2*qz*qz + 1)),
            2*qr*(-2*qy*qy - 2*qz*qz + 1)/(sq(2*qr*qz + 2*qx*qy) + sq(-2*qy*qy - 2*qz*qz + 1)) - 4*qz*(-2*qr*qz - 2*qx*qy)/(sq(2*qr*qz + 2*qx*qy) + sq(-2*qy*qy - 2*qz*qz + 1)),

            2*qy/sqrt(-sq(2*qr*qy - 2*qx*qz) + 1),
            -2*qz/sqrt(-sq(2*qr*qy - 2*qx*qz) + 1),
            2*qr/sqrt(-sq(2*qr*qy - 2*qx*qz) + 1),
            -2*qx/sqrt(-sq(2*qr*qy - 2*qx*qz) + 1),

            2*qx*(-2*qx*qx - 2*qy*qy + 1)/(sq(2*qr*qx + 2*qy*qz) + sq(-2*qx*qx - 2*qy*qy + 1)),
            2*qr*(-2*qx*qx - 2*qy*qy + 1)/(sq(2*qr*qx + 2*qy*qz) + sq(-2*qx*qx - 2*qy*qy + 1)) - 4*qx*(-2*qr*qx - 2*qy*qz)/(sq(2*qr*qx + 2*qy*qz) + sq(-2*qx*qx - 2*qy*qy + 1)),
            -4*qy*(-2*qr*qx - 2*qy*qz)/(sq(2*qr*qx + 2*qy*qz) + sq(-2*qx*qx - 2*qy*qy + 1)) + 2*qz*(-2*qx*qx - 2*qy*qy + 1)/(sq(2*qr*qx + 2*qy*qz) + sq(-2*qx*qx - 2*qy*qy + 1)),
            2*qy*(-2*qx*qx - 2*qy*qy + 1)/(sq(2*qr*qx + 2*qy*qz) + sq(-2*qx*qx - 2*qy*qy + 1));

        // clang-format on
    }

    return m;
}

/// the jacobian of p7 to p6 conversion
/// Equation (2.12)
Matrix6x7 jacobian_p7_to_p6_wrt_p(const Vector7 &p) {
    Matrix6x7 r = Matrix6x7::Zero();

    // Equation (2.12)
    // bottom left 3x3 is zero
    // top right 3x4 is zero
    // top left 3x3 is identity
    r(0, 0) = 1;
    r(1, 1) = 1;
    r(2, 2) = 1;

    // This function takes the un-normalized quaternion
    Matrix4x4 jacobian_quat_norm =
      jacobian_Quat_Norm_wrt_q(p.block<4, 1>(3, 0));

    Matrix3x4 jacobian_quat_norm_to_rpy =
      jacobian_Quat_Norm_to_Rpy_wrt_q(p.block<4, 1>(3, 0));

    r.block<3, 4>(3, 3) = jacobian_quat_norm_to_rpy * jacobian_quat_norm;

    return r;
}

// jacobian of composing a point to a p7
// Equation (3.8)
Matrix3x7 jacobian_p7_Point_Composition_wrt_p(const Vector7 &p,
                                              const Vector3 &a) {
    Matrix3x7 m = Matrix3x7::Zero();

    // The 3x3 on the top left is the identity matrix
    m.block<3, 3>(0, 0) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;

    // Equation (3.9)
    double ax = a(0), ay = a(1), az = a(2);
    double qr = p(3), qx = p(4), qy = p(5), qz = p(6);

    // clang-format off
    m.block<3, 4>(0, 3) <<
        -qz*ay+qy*az, qy*ay+qz*az, -2*qy*ax+qx*ay+qr*az, -2*qz*ax-qr*ay+qx*az,
        qz*ax-qx*az, qy*ax-2*qx*ay-qr*az, qx*ax+qz*az, qr*ax-2*qz*ay+qy*az,
        -qy*ax+qx*ay, qz*ax+qr*ay-2*qx*az, -qr*ax+qz*ay-2*qy*az, qx*ax+qy*ay;
    // clang-format on

    m.block<3, 4>(0, 3) *= 2;

    // applying jacobian normalization
    Matrix4x4 jacobian_quat_norm =
      jacobian_Quat_Norm_wrt_q(p.block<4, 1>(3, 0));
    m.block<3, 4>(0, 3) *= jacobian_quat_norm;

    return m;
}

// jacobian of the composition of p7 poses
// Equation (5.8)
Matrix7x7 jacobian_p7_p7_Composition_wrt_p1(const Vector7 &p1,
                                            const Vector7 &p2) {
    Matrix7x7 m = Matrix7x7::Zero();

    Vector4 q2 = p2.block<4, 1>(3, 0);
    double qr2 = q2(0), qx2 = q2(1), qy2 = q2(2), qz2 = q2(3);

    // clang-format off
    m.block<4, 4>(3, 3) << qr2, -qx2, -qy2, -qz2,
                           qx2, qr2, qz2, -qy2,
                           qy2, -qz2, qr2, qx2,
                           qz2, qy2, -qx2, qr2;
    // clang-format on

    // Note: this quaternion normalization jacobian matrix is not present in
    // the book's formulation, but it should be there if a jacobian
    // normalization is performed on the input
    Matrix4x4 jacobian_quat_norm =
      jacobian_Quat_Norm_wrt_q(p1.block<4, 1>(3, 0));
    m.block<4, 4>(3, 3) *= jacobian_quat_norm;

    m.block<3, 7>(0, 0) =
      jacobian_p7_Point_Composition_wrt_p(p1, p2.block<3, 1>(0, 0));

    return m;
}

// jacobian of composing a point to a p7
// Equation (3.10)
Matrix3x3 jacobian_p7_Point_Composition_wrt_a(const Vector7 &p,
                                              const Vector3 &a) {
    Matrix3x3 m = Matrix3x3::Zero();

    double qr = p(3), qx = p(4), qy = p(5), qz = p(6);

    // clang-format off
    m << 0.5 - qy*qy - qz*qz , qx*qy - qr*qz, qr*qy + qx*qz,
         qr*qz + qx*qy, 0.5 - qx*qx - qz*qz, qy*qz - qr*qx,
         qx*qz - qr*qy, qr*qx + qy*qz, 0.5 - qx*qx - qy*qy;
    // clang-format on

    m *= 2;

    // To avoid warnings
    (void) a;

    return m;
}

// jacobian of the composition of p7 poses
// Equation (5.9)
Matrix7x7 jacobian_p7_p7_Composition_wrt_p2(const Vector7 &p1,
                                            const Vector7 &p2) {
    Matrix7x7 m = Matrix7x7::Zero();

    Vector4 q1 = p1.block<4, 1>(3, 0);
    double qr1 = q1(0), qx1 = q1(1), qy1 = q1(2), qz1 = q1(3);

    // clang-format off
    m.block<4, 4>(3, 3) << qr1, -qx1, -qy1, -qz1,
                           qx1, qr1, -qz1, qy1,
                           qy1, qz1, qr1, -qx1,
                           qz1, -qy1, qx1, qr1;
    // clang-format on

    // Note: this quaternion normalization jacobian matrix is not present in
    // the book's formulation, but it should be there if a jacobian
    // normalization is performed on the input
    Matrix4x4 jacobian_quat_norm =
      jacobian_Quat_Norm_wrt_q(p2.block<4, 1>(3, 0));
    m.block<4, 4>(3, 3) *= jacobian_quat_norm;

    m.block<3, 3>(0, 0) =
      jacobian_p7_Point_Composition_wrt_a(p1, p2.block<3, 1>(0, 0));

    return m;
}

// jacobian of converting a p6 to a p7
// Equation (2.8)
Matrix7x6 jacobian_p6_to_p7_wrt_p(const Vector6 &p) {
    Matrix7x6 m = Matrix7x6::Zero();

    // top left corner is a identity matrix
    // the bottom left 4x3 and top right 3x3 is zero
    m.block<3, 3>(0, 0) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;

    double ccc, ccs, csc, scc, ssc, sss, scs, css;
    double roll = p(5), pitch = p(4), yaw = p(3);
    ccc = cos(roll / 2) * cos(pitch / 2) * cos(yaw / 2);
    ccs = cos(roll / 2) * cos(pitch / 2) * sin(yaw / 2);
    csc = cos(roll / 2) * sin(pitch / 2) * cos(yaw / 2);
    scs = sin(roll / 2) * cos(pitch / 2) * sin(yaw / 2);
    css = cos(roll / 2) * sin(pitch / 2) * sin(yaw / 2);
    scc = sin(roll / 2) * cos(pitch / 2) * cos(yaw / 2);
    ssc = sin(roll / 2) * sin(pitch / 2) * cos(yaw / 2);
    sss = sin(roll / 2) * sin(pitch / 2) * sin(yaw / 2);

    // clang-format off
    m.block<4, 3>(3, 3) <<
        (ssc - ccs) / 2.0, (scs - csc) / 2.0, (css - scc) / 2.0,
        -(csc + scs) / 2.0, -(ssc + ccs) / 2.0, (ccc + sss) / 2.0,
        (scc - css) / 2.0, (ccc - sss) / 2.0, (ccs - ssc) / 2.0,
        (ccc + sss) / 2.0, -(css + scc) / 2.0, -(csc + scs) / 2.0;
    // clang-format on

    return m;
}
}  // namespace wave

namespace wave {
namespace pose_comp {

Vector3 quatToYPR(const Eigen::Quaterniond &q) {
    Vector3 v;
    double qr = q.coeffs().w(), qx = q.coeffs().x(), qy = q.coeffs().y(),
           qz = q.coeffs().z();
    double delta = qr * qy - qx * qz;  // discriminant of normalized quaternion
    double roll, pitch, yaw;

    // Check for special cases when abs(delta) =~ 0.5 (Equation (2.10))
    if (fabs(delta - 0.5) < 1e-6) {
        yaw = -2 * atan2(qx, qr);
        pitch = M_PI / 2;
        roll = 0;

    } else if (fabs(delta + 0.5) < 1e-6) {
        yaw = 2 * atan2(qx, qr);
        pitch = -M_PI / 2;
        roll = 0;
    } else {
        yaw = atan2(2 * (qr * qz + qx * qy), (1 - 2 * (qy * qy + qz * qz)));
        pitch = asin(2 * delta);
        roll = atan2(2 * (qr * qx + qy * qz), (1 - 2 * (qx * qx + qy * qy)));
    }

    v(0) = yaw;
    v(1) = pitch;
    v(2) = roll;

    return v;
}

Eigen::Quaterniond yprToQuat(const Vector3 &ypr) {
    Eigen::Quaterniond q;
    double qr, qx, qy, qz;
    double half_y = ypr(0) / 2, half_p = ypr(1) / 2, half_r = ypr(2) / 2;
    double cy = cos(half_y), cp = cos(half_p), cr = cos(half_r),
           sy = sin(half_y), sp = sin(half_p), sr = sin(half_r);

    qr = cr * cp * cy + sr * sp * sy;
    qx = sr * cp * cy - cr * sp * sy;
    qy = cr * sp * cy + sr * cp * sy;
    qz = cr * cp * sy - sr * sp * cy;

    q.coeffs() << qx, qy, qz, qr;

    return q;
}

Matrix3x3 yprToRotMatrix(const Vector3 &ypr) {
    double y = ypr(0), p = ypr(1), r = ypr(2);
    Matrix3x3 rotMatrix;
    double cy = cos(y), cp = cos(p), cr = cos(r), sy = sin(y), sp = sin(p),
           sr = sin(r);

    rotMatrix << cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr,
      sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, -sp, cp * sr,
      cp * cr;

    return rotMatrix;
}

Vector3 rotMatrixToYPR(const Matrix3x3 &p) {
    Vector3 ypr;
    double yaw, pitch, roll;
    double p11 = p(0, 0), p12 = p(0, 1), p13 = p(0, 2), p21 = p(1, 0),
           p22 = p(1, 1), p23 = p(1, 2), p31 = p(2, 0), p32 = p(2, 1),
           p33 = p(2, 2);

    pitch = atan2(-p31, sqrt(p11 * p11 + p21 * p21));

    if (fabs(pitch + M_PI * 1.0 / 2) < 1e-6) {
        yaw = atan2(-p23, -p13);
        roll = 0;
    } else if (fabs(pitch - M_PI * 1.0 / 2) < 1e-6) {
        yaw = atan2(p23, p13);
        roll = 0;
    } else {
        yaw = atan2(p21, p11);
        roll = atan2(p32, p33);
    }

    ypr << yaw, pitch, roll;

    // To avoid warnings
    (void) p12;
    (void) p22;

    return ypr;
}

Eigen::Quaterniond rotMatrixToQuat(const Matrix3x3 &p) {
    Eigen::Quaterniond q;
    Vector3 ypr;

    ypr = rotMatrixToYPR(p);
    q = yprToQuat(ypr);

    return q;
}
}  // namespace pose_comp
}  // namespace wave


///////////////////////////////////////////////////////////////
//  Copyright 2011-9 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#define BOOST_CHRONO_HEADER_ONLY

#ifdef _MSC_VER
#define _SCL_SECURE_NO_WARNINGS
#endif

#if !defined(TEST_MPF) && !defined(TEST_MPZ) && \
    !defined(TEST_CPP_DEC_FLOAT) && !defined(TEST_MPFR) && !defined(TEST_MPQ) && !defined(TEST_TOMMATH) && \
    !defined(TEST_TOMMATH_BOOST_RATIONAL) && !defined(TEST_MPZ_BOOST_RATIONAL) && !defined(TEST_CPP_INT) && \
    !defined(TEST_CPP_INT_RATIONAL) && !defined(TEST_CPP_BIN_FLOAT)
#define TEST_MPF
#define TEST_MPZ
#define TEST_MPQ
#define TEST_MPFR
#define TEST_CPP_DEC_FLOAT
#define TEST_MPQ
#define TEST_TOMMATH
#define TEST_CPP_INT
#define TEST_CPP_INT_RATIONAL
#define TEST_CPP_BIN_FLOAT

#ifdef _MSC_VER
#pragma message("CAUTION!!: No backend type specified so testing everything.... this will take some time!!")
#endif
#ifdef __GNUC__
#pragma warning "CAUTION!!: No backend type specified so testing everything.... this will take some time!!"
#endif

#endif

#include <boost/chrono.hpp>
#include <vector>
#include <map>
#include <string>
#include <cstring>
#include <cctype>
#include <iostream>
#include <iomanip>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/multiprecision/number.hpp>

template <class Clock>
struct stopwatch
{
   typedef typename Clock::duration duration;
   stopwatch()
   {
      m_start = Clock::now();
   }
   duration elapsed()
   {
      return Clock::now() - m_start;
   }
   void reset()
   {
      m_start = Clock::now();
   }

 private:
   typename Clock::time_point m_start;
};

extern unsigned bits_wanted; // for integer types

template <class T, int Type>
struct tester
{
   tester()
   {
      a.assign(500, 0);
      for (int i = 0; i < 500; ++i)
      {
         b.push_back(generate_random());
         c.push_back(generate_random());
         small.push_back(gen());
      }
   }
   double test_add()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j] + c[j];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_subtract()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j] - c[j];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_add_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j] + 1;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_subtract_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j] - 1;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_multiply()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned k = 0; k < b.size(); ++k)
            a[k] = b[k] * c[k];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_multiply_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j] * 3;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_divide()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j] / c[j] + b[j] / small[j];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_divide_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j] / 3;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_str(const std::integral_constant<bool, false>&)
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < b.size(); ++i)
         a[i] = boost::lexical_cast<T>(boost::lexical_cast<std::string>(b[i]));
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_str(const std::integral_constant<bool, true>&)
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < b.size(); ++i)
         a[i].assign(b[i].str());
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_str()
   {
      return test_str(std::is_class<T>());
   }
   //
   // The following tests only work for integer types:
   //
   double test_mod()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] % c[i] + b[i] % small[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_mod_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] % 254;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_or()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] | c[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_or_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] | 234;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_and()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] & c[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_and_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] & 234;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_xor()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] ^ c[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_xor_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] ^ 234;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_complement()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            a[i] = ~b[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_left_shift()
   {
      int                                             max_shift = std::numeric_limits<T>::is_bounded ? std::numeric_limits<T>::digits : bits_wanted;
      int                                             shift     = 0;
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] << (shift++ % max_shift);
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_right_shift()
   {
      int                                             max_shift = 2 + std::numeric_limits<T>::is_bounded ? std::numeric_limits<T>::digits : bits_wanted;
      int                                             shift     = 0;
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] >> (shift++) % max_shift;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_gcd()
   {
      using boost::integer::gcd;
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            a[i] = gcd(b[i], c[i]);
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_powm()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 25; ++i)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            a[i] = powm(b[i], b[i] / 2, c[i]);
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_construct()
   {
      std::allocator<T>                               alloc;
      T*                                              pt = alloc.allocate(1000);
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < 1000; ++j)
            new (pt + j) T();
         for (unsigned j = 0; j < 1000; ++j)
            std::allocator_traits<std::allocator<T> >::destroy(alloc, pt + j);
      }
      double result = boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
      alloc.deallocate(pt, 1000);
      return result;
   }
   double test_construct_unsigned()
   {
      std::allocator<T>                               alloc;
      T*                                              pt = alloc.allocate(1000);
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < 1000; ++j)
            new (pt + j) T(j);
         for (unsigned j = 0; j < 1000; ++j)
            std::allocator_traits<std::allocator<T> >::destroy(alloc, pt + j);
      }
      double result = boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
      alloc.deallocate(pt, 1000);
      return result;
   }
   double test_construct_unsigned_ll()
   {
      std::allocator<T>                               alloc;
      T*                                              pt = alloc.allocate(1000);
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned long long j = 0; j < 1000; ++j)
            new (pt + j) T(j);
         for (unsigned j = 0; j < 1000; ++j)
            std::allocator_traits<std::allocator<T>  >::destroy(alloc, pt + j);
      }
      double result = boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
      alloc.deallocate(pt, 1000);
      return result;
   }

   //
   // Hetero operations:
   //
   template <class U>
   static U get_hetero_test_value(std::integral_constant<bool, false> const&)
   {
      return U(2) / 3;
   }
   template <class U>
   static U get_hetero_test_value(std::integral_constant<bool, true> const&)
   {
      return (std::numeric_limits<U>::max)() >> 4;
   }
   template <class U>
   static U get_hetero_test_value()
   {
      return get_hetero_test_value<U>(boost::multiprecision::detail::is_integral<U>());
   }
   template <class U>
   double test_multiply_hetero()
   {
      static const U                                  val = get_hetero_test_value<U>();
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j] * val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   template <class U>
   double test_inplace_multiply_hetero()
   {
      static const U val = get_hetero_test_value<U>();
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j], a[j] *= val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   template <class U>
   double test_add_hetero()
   {
      static const U                                  val = get_hetero_test_value<U>();
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j] + val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   template <class U>
   double test_inplace_add_hetero()
   {
      static const U val = get_hetero_test_value<U>();
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j], a[j] += val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   template <class U>
   double test_subtract_hetero()
   {
      static const U                                  val = get_hetero_test_value<U>();
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j] - val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   template <class U>
   double test_inplace_subtract_hetero()
   {
      static const U val = get_hetero_test_value<U>();
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j], a[j] -= val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   template <class U>
   double test_divide_hetero()
   {
      static const U                                  val = get_hetero_test_value<U>();
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j] / val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   template <class U>
   double test_inplace_divide_hetero()
   {
      static const U val = get_hetero_test_value<U>();
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned i = 0; i < 1000; ++i)
      {
         for (unsigned j = 0; j < b.size(); ++j)
            a[j] = b[j], a[j] /= val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }

 private:
   T generate_random()
   {
      return generate_random(std::integral_constant<int, Type>());
   }
   T generate_random(const std::integral_constant<int, boost::multiprecision::number_kind_floating_point>&)
   {
      T val      = gen();
      T prev_val = -1;
      while (val != prev_val)
      {
         val *= (gen.max)();
         prev_val = val;
         val += gen();
      }
      int e;
      val = frexp(val, &e);

      typedef typename T::backend_type::exponent_type        e_type;
      static boost::random::uniform_int_distribution<e_type> ui(-30, 30);
      return ldexp(val, static_cast<int>(ui(gen)));
   }
   T generate_random(const std::integral_constant<int, boost::multiprecision::number_kind_integer>&)
   {
      typedef boost::random::mt19937::result_type random_type;

      T        max_val;
      unsigned digits;
      if (std::numeric_limits<T>::is_bounded)
      {
         max_val = (std::numeric_limits<T>::max)();
         digits  = std::numeric_limits<T>::digits;
      }
      else
      {
         max_val = T(1) << bits_wanted;
         digits  = bits_wanted;
      }

      unsigned bits_per_r_val = std::numeric_limits<random_type>::digits - 1;
      while ((random_type(1) << bits_per_r_val) > (gen.max)())
         --bits_per_r_val;

      unsigned terms_needed = digits / bits_per_r_val + 1;

      T val = 0;
      for (unsigned i = 0; i < terms_needed; ++i)
      {
         val *= (gen.max)();
         val += gen();
      }
      val %= max_val;
      return val;
   }
   T generate_random(const std::integral_constant<int, boost::multiprecision::number_kind_rational>&)
   {
      typedef boost::random::mt19937::result_type                     random_type;
      typedef typename boost::multiprecision::component_type<T>::type IntType;

      IntType  max_val;
      unsigned digits;
      if (std::numeric_limits<IntType>::is_bounded)
      {
         max_val = (std::numeric_limits<IntType>::max)();
         digits  = std::numeric_limits<IntType>::digits;
      }
      else
      {
         max_val = IntType(1) << bits_wanted;
         digits  = bits_wanted;
      }

      unsigned bits_per_r_val = std::numeric_limits<random_type>::digits - 1;
      while ((random_type(1) << bits_per_r_val) > (gen.max)())
         --bits_per_r_val;

      unsigned terms_needed = digits / bits_per_r_val + 1;

      IntType val   = 0;
      IntType denom = 0;
      for (unsigned i = 0; i < terms_needed; ++i)
      {
         val *= (gen.max)();
         val += gen();
      }
      for (unsigned i = 0; i < terms_needed; ++i)
      {
         denom *= (gen.max)();
         denom += gen();
      }
      if (denom == 0)
         denom = 1;
      val %= max_val;
      denom %= max_val;
      return T(val, denom);
   }
   std::vector<T>                a, b, c, small;
   static boost::random::mt19937 gen;
};

template <class N, int V>
boost::random::mt19937 tester<N, V>::gen;

inline const char* category_name(const std::integral_constant<int, boost::multiprecision::number_kind_integer>&)
{
   return "integer";
}
inline const char* category_name(const std::integral_constant<int, boost::multiprecision::number_kind_floating_point>&)
{
   return "float";
}
inline const char* category_name(const std::integral_constant<int, boost::multiprecision::number_kind_rational>&)
{
   return "rational";
}

//
// Keys in order are:
// Category
// Operator
// Type
// Precision
// Time
//
extern std::map<std::string, std::map<std::string, std::map<std::string, std::map<int, double> > > > result_table;

inline void report_result(const char* cat, const char* type, const char* op, unsigned precision, double time)
{
   std::cout << std::left << std::setw(15) << type << std::setw(10) << precision << std::setw(35) << op << time << std::endl;
   result_table[cat][op][type][precision] = time;
}

template <class Number, int N>
void test_int_ops(tester<Number, N>& t, const char* type, unsigned precision, const std::integral_constant<int, boost::multiprecision::number_kind_integer>&)
{
   const char* cat = "integer";
   report_result(cat, type, "%", precision, t.test_mod());
   report_result(cat, type, "|", precision, t.test_or());
   report_result(cat, type, "&", precision, t.test_and());
   report_result(cat, type, "^", precision, t.test_xor());
   //report_result(cat, type, "~", precision, t.test_complement());
   report_result(cat, type, "<<", precision, t.test_left_shift());
   report_result(cat, type, ">>", precision, t.test_right_shift());
   // integer ops:
   report_result(cat, type, "%(int)", precision, t.test_mod_int());
   report_result(cat, type, "|(int)", precision, t.test_or_int());
   report_result(cat, type, "&(int)", precision, t.test_and_int());
   report_result(cat, type, "^(int)", precision, t.test_xor_int());
   report_result(cat, type, "gcd", precision, t.test_gcd());
   if(precision <= 1024)
      report_result(cat, type, "powm", precision, t.test_powm());
}
template <class Number, int N, class U>
void test_int_ops(tester<Number, N>&, const char*, unsigned, const U&)
{
}

template <class Number>
void test(const char* type, unsigned precision)
{
   bits_wanted = precision;
   tester<Number, boost::multiprecision::number_category<Number>::value> t;
   const char*                                                           cat = category_name(typename boost::multiprecision::number_category<Number>::type());
   //
   // call t.test_multiply() first so that the destination operands are
   // forced to perform whatever memory allocation may be needed.  That way
   // we measure only algorithm performance, and not memory allocation effects.
   //
   t.test_multiply();
   //
   // Now the actual tests:
   //
#ifndef TEST_MUL_ONLY
   report_result(cat, type, "+", precision, t.test_add());
   report_result(cat, type, "-", precision, t.test_subtract());
#endif
   report_result(cat, type, "*", precision, t.test_multiply());
#ifndef TEST_MUL_ONLY
   report_result(cat, type, "/", precision, t.test_divide());
   report_result(cat, type, "str", precision, t.test_str());
   // integer ops:
   report_result(cat, type, "+(int)", precision, t.test_add_int());
   report_result(cat, type, "-(int)", precision, t.test_subtract_int());
   report_result(cat, type, "*(int)", precision, t.test_multiply_int());
   report_result(cat, type, "/(int)", precision, t.test_divide_int());
   // construction and destruction:
   report_result(cat, type, "construct", precision, t.test_construct());
   report_result(cat, type, "construct(unsigned)", precision, t.test_construct_unsigned());
   report_result(cat, type, "construct(unsigned long long)", precision, t.test_construct_unsigned_ll());
   test_int_ops(t, type, precision, typename boost::multiprecision::number_category<Number>::type());
   // Hetero ops:
   report_result(cat, type, "+(unsigned long long)", precision, t.template test_add_hetero<unsigned long long>());
   report_result(cat, type, "-(unsigned long long)", precision, t.template test_subtract_hetero<unsigned long long>());
   report_result(cat, type, "*(unsigned long long)", precision, t.template test_multiply_hetero<unsigned long long>());
   report_result(cat, type, "/(unsigned long long)", precision, t.template test_divide_hetero<unsigned long long>());
   report_result(cat, type, "+=(unsigned long long)", precision, t.template test_inplace_add_hetero<unsigned long long>());
   report_result(cat, type, "-=(unsigned long long)", precision, t.template test_inplace_subtract_hetero<unsigned long long>());
   report_result(cat, type, "*=(unsigned long long)", precision, t.template test_inplace_multiply_hetero<unsigned long long>());
   report_result(cat, type, "/=(unsigned long long)", precision, t.template test_inplace_divide_hetero<unsigned long long>());
#endif
}

void test01();
void test02();
void test03();
void test04();
void test05();
void test06();
void test07();
void test08();
void test09();
void test10();
void test11();
void test12();
void test13();
void test14();
void test15();
void test16();
void test17();
void test18();
void test19();
void test20();
void test21();
void test22();
void test23();
void test24();
void test25();
void test26();
void test27();
void test28();
void test29();
void test30();
void test31();
void test32();
void test33();
void test34();
void test35();
void test36();
void test37();
void test38();
void test39();
void test40();
void test41();
void test42();
void test43();
void test44();
void test45();
void test46();
void test47();
void test48();
void test49();
void test50();
void test51();


#ifndef INCLUDED_objective_voxel_FieldCache_HH
#define INCLUDED_objective_voxel_FieldCache_HH

#include "scheme/util/assert.hh"
#include "scheme/objective/voxel/VoxelArray.hh"
#include "scheme/io/cache.hh"
// #include <boost/exception/all.hpp>
#include <exception>

namespace scheme { namespace objective { namespace voxel {

struct FieldException : public std::runtime_error {
	FieldException(std::string m) : std::runtime_error(m.c_str()) {}
};

template<class Float=float>
struct Field3D {
	virtual Float operator()(Float f, Float g, Float h) const = 0;
};

template<class Float=float>
struct FieldCache3D : public VoxelArray<3,Float> {
	typedef VoxelArray<3,Float> BASE;
	typedef typename BASE::Bounds Float3;
	typedef typename VoxelArray<3,Float>::Indices Indices;

	std::string cache_loc_;

	FieldCache3D() : cache_loc_("") {}

	template<class F1,class F2, class F3>
	FieldCache3D(
		Field3D<Float> const & field,
		F1 const & lb,
		F2 const & ub,
		F3 const & cs,
		std::string const & cache_loc="",
		bool no_init = false,
		int oversample=1
	) : BASE(lb,ub,cs), cache_loc_(cache_loc)
	{
		typename BASE::Indices extents;
		for(size_t i = 0; i < BASE::DIM; ++i) extents[i] = this->shape()[i];
		#ifdef CEREAL
			if( io::read_cache(cache_loc_,*this) ){
				bool extents_eq = true;
				for(size_t i = 0; i < BASE::DIM; ++i) extents_eq &= extents[i] == this->shape()[i];
				if( Float3(lb)==this->lb_ && Float3(ub)==this->ub_ && Float3(cs)==this->cs_ && extents_eq ){
					std::cout << "EXTENTS EQ, USING CACHE" << std::endl;
					check_against_field( field );
					return;
				}
				std::cout << "Warning, FieldCache3D bad cache " << cache_loc_ << " bounds mismatch, recomputing..." << std::endl;
				this->resize(extents);
			} // else {
		#endif
		// 	std::cout << "NO CACHE" << std::endl;
		// }
		if( !no_init ){
			for(int k = 0; k < this->shape()[2]; ++k){
			for(int j = 0; j < this->shape()[1]; ++j){
			for(int i = 0; i < this->shape()[0]; ++i){
				Float3 cen = this->indices_to_center( Indices(i,j,k) );
				this->operator[]( cen ) = this->sample_field( field, cen, oversample );
				// std::exit(-1);
			}}}
		}
		#ifdef CEREAL
			io::write_cache(cache_loc_,*this);
		#endif

	}

	Float
	sample_field(
		Field3D<Float> const & field,
		Float3 cen,
		int oversample
	) const {
		Float const om = 1.0/oversample;
		Float const oo = om/2.0 - 0.5;
		Float mn = std::numeric_limits<Float>::max();
		for(int o = 0; o < oversample; ++o){
		for(int p = 0; p < oversample; ++p){
		for(int q = 0; q < oversample; ++q){
			Float ox = cen[0] + ( o*om + oo ) * this->cs_[0];
			Float oy = cen[1] + ( p*om + oo ) * this->cs_[1];
			Float oz = cen[2] + ( q*om + oo ) * this->cs_[2];
			// std::cout << ox << " " << oy << " " << oz << " " << cen[0] << " " << cen[1] << " " << cen[2] << std::endl;
			Float fval = field( ox, oy, oz );
			mn = std::min( mn, fval );
		}}}
		return mn;
	}

	double check_against_field( Field3D<Float> const & field, int oversample=1, float tolerance=0.0001 ) const {
		int nerror(0), nsamp(0);
		{
			// sanity check
			// std::exit(-1);
			// std::cout << this->shape()[0] << std::endl;
			// std::cout << this->shape()[1] << std::endl;
			// std::cout << this->shape()[2] << std::endl;
			for(int i = 0; i < this->shape()[0]; i += 8){
			for(int j = 0; j < this->shape()[1]; j += 8){
			for(int k = 0; k < this->shape()[2]; k += 8){
				++nsamp;
				Float3 cen = this->indices_to_center( Indices(i,j,k) );
				Float test1 = this->operator[]( cen );
				// Float test2 = field( cen[0], cen[1], cen[2] );
				Float test2 = sample_field(field,cen,oversample);
				if (test1 > 99 && test2 > 99) {
					continue;
				}
				if( fabs(test1-test2) > tolerance ){
					std::cout << "FIELD MISMATCH stored: " << test1 << " recalculated: " << test2 << std::endl;
					++nerror;
					// if(!permissive) throw FieldException("field check fails");
				} else {

				}
			}}}
		}
		// if( nerror==0 ){
		// 	#ifdef USE_OPENMP
		// 	#pragma omp critical
		// 	#endif
		// 	std::cout << "    check_against_field pass " << nsamp-nerror << " of " << nsamp << std::endl;
		// }
		return (double)nerror/(double)nsamp;
	}

};

template<class Float> struct AggMin {
	static Float initval() { return std::numeric_limits<Float>::max(); }
	static Float aggregate( Float agg, Float newval ) { return std::min(agg,newval); }
};
template<class Float> struct AggMax {
	static Float initval() { return std::numeric_limits<Float>::min(); }
	static Float aggregate( Float agg, Float newval ) { return std::max(agg,newval); }
};

template<class Float=float,template<class> class AGG = AggMin>
struct BoundingFieldCache3D : public VoxelArray<3,Float> {
	typedef VoxelArray<3,Float> BASE;
	typedef AGG<Float> Aggregator;
	typedef typename BASE::Bounds Float3;
	// Float spread_;
	// std::string cache_loc_;
	template<class F>
	BoundingFieldCache3D(
		VoxelArray<3,Float> const & ref,
		Float spread,
		F const & cs,
		std::string cache_loc="",
		bool no_init = false
	) : BASE(ref.lb_-spread,ref.ub_+spread,cs) //,
	    // ref_(ref),
	    // spread_(spread),
	    // cache_loc_(cache_loc)
	{
		if( ref.cs_.norm() > spread )
			std::cout << "BoundingFieldCache3D warning: spread " << spread
		              << " less than ref cell_size.norm() " << ref.cs_.norm() << std::endl;
//		Float3 lb = ref.lb_-spread;
//		Float3 ub = ref.ub_+spread;
		typename BASE::Indices extents;
		for(size_t i = 0; i < BASE::DIM; ++i) extents[i] = this->shape()[i];
		#ifdef CEREAL
			if( io::read_cache(cache_loc,*this) ){
				bool extents_eq = true;
				for(size_t i = 0; i < BASE::DIM; ++i) extents_eq &= extents[i] == this->shape()[i];
				if( Float3(lb)==this->lb_ && Float3(ub)==this->ub_ && Float3(cs)==this->cs_ && extents_eq ) return;
				std::cout << "Warning, BoundingFieldCache3D bad cache " << cache_loc << " bounds mismatch, recomputing..." << std::endl;
				this->resize(extents);
			}
		#endif
		if( !no_init ){
			// size_t ncalls = 0;
			for(Float h = this->lb_[2]+this->cs_[2]/2.0; h < this->ub_[2]+this->cs_[2]/2.0; h += this->cs_[2]){
			for(Float g = this->lb_[1]+this->cs_[1]/2.0; g < this->ub_[1]+this->cs_[1]/2.0; g += this->cs_[1]){
			for(Float f = this->lb_[0]+this->cs_[0]/2.0; f < this->ub_[0]+this->cs_[0]/2.0; f += this->cs_[0]){
				// ++ncalls;
				this->operator[]( Float3(f,g,h) ) = calc_agg_val( ref, spread, Float3(f,g,h) );
			}}}
		}
		#ifdef CEREAL
			io::write_cache(cache_loc,*this);
		#endif
	}
	Float calc_agg_val(
		VoxelArray<3,Float> const & ref,
		float spread,
		Float3 const & f3
	) const {
		Float3 beg = ref.lb_.max(f3-spread) + ref.cs_/2.0;
		Float3 end = ref.ub_.min(f3+spread);
		Float3 idx;
		Float val = Aggregator::initval();
		int count = 0;
		for(idx[0] = beg[0]; idx[0] < end[0]; idx[0] += ref.cs_[0]){
		for(idx[1] = beg[1]; idx[1] < end[1]; idx[1] += ref.cs_[1]){
		for(idx[2] = beg[2]; idx[2] < end[2]; idx[2] += ref.cs_[2]){
			if( (f3-idx).squaredNorm() <= spread*spread ){
				val = Aggregator::aggregate( val, ref[idx] );
				++count;
			}
		}}}
		// std::cout << f3 << " | " << beg << " => " << end << std::endl;
		return count==0 ? 0.0 : val;
	}
};

}}}

#endif


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the deep learning tools from the
    dlib C++ Library.  I'm assuming you have already read the dnn_introduction_ex.cpp and
    the dnn_introduction2_ex.cpp examples.  So in this example program I'm going to go
    over a transfer learning example, which includes:
        - Defining a layer visitor to modify the some network parameters for fine-tuning
        - Using pretrained layers of a network for another task
*/

#include <dlib/dnn.h>
#include <iostream>

// This header file includes a generic definition of the most common ResNet architectures
#include "resnet.h"

using namespace std;
using namespace dlib;

// In this simple example we will show how to load a pretrained network and use it for a
// different task.  In particular, we will load a ResNet50 trained on ImageNet, adjust
// some of its parameters and use it as a pretrained backbone for some metric learning
// task.

// Let's start by defining a network that will use the ResNet50 backbone from resnet.h
namespace model
{
    template<template<typename> class BN>
    using net_type = loss_metric<
        fc_no_bias<128,
        avg_pool_everything<
        typename resnet::def<BN>::template backbone_50<
        input_rgb_image
        >>>>;

    using train = net_type<bn_con>;
    using infer = net_type<affine>;
}

// Next, we define a layer visitor that will modify the weight decay of a network.  The
// main interest of this class is to show how one can define custom visitors that modify
// some network parameters.
class visitor_weight_decay_multiplier
{
public:

    visitor_weight_decay_multiplier(double new_weight_decay_multiplier_) :
        new_weight_decay_multiplier(new_weight_decay_multiplier_) {}

    template<typename input_layer_type>
    void operator()(size_t , input_layer_type& )  const
    {
        // ignore other layers
    }

    template <typename T, typename U, typename E>
    void operator()(size_t , add_layer<T,U,E>& l)  const
    {
        set_weight_decay_multiplier(l.layer_details(), new_weight_decay_multiplier);
    }

private:

    double new_weight_decay_multiplier;
};

int main() try
{
    // Let's instantiate our network in train mode.
    model::train net;

    // We create a new scope so that resources from the loaded network are freed
    // automatically when leaving the scope.
    {
        // Now, let's define the classic ResNet50 network and load the pretrained model on
        // ImageNet.
        resnet::train_50 resnet50;
        std::vector<string> labels;
        deserialize("resnet50_1000_imagenet_classifier.dnn") >> resnet50 >> labels;

        // For transfer learning, we are only interested in the ResNet50's backbone, which
        // lays below the loss and the fc layers, so we can extract it as:
        auto backbone = std::move(resnet50.subnet().subnet());

        // We can now assign ResNet50's backbone to our network skipping the different
        // layers, in our case, the loss layer and the fc layer:
        net.subnet().subnet() = backbone;

        // An alternative way to use the pretrained network on a different
        // network is to extract the relevant part of the network (we remove
        // loss and fc layers), stack the new layers on top of it and assign
        // the network.
        using net_type = loss_metric<fc_no_bias<128, decltype(backbone)>>;
        net_type net2;
        net2.subnet().subnet() = backbone;
    }

    // We can use the visit_layers function to modify the weight decay of the entire
    // network:
    visit_layers(net, visitor_weight_decay_multiplier(0.001));

    // We can also use predefined visitors to affect the learning rate of the whole
    // network.
    set_all_learning_rate_multipliers(net, 0.5);

    // Modifying the learning rates of a network is a common practice for fine tuning, for
    // this reason it is already provided. However, it is implemented internally using a
    // visitor that is very similar to the one defined in this example.

    // Usually, we want to freeze the network, except for the top layers:
    visit_layers(net.subnet().subnet(), visitor_weight_decay_multiplier(0));
    set_all_learning_rate_multipliers(net.subnet().subnet(), 0);

    // Alternatively, we can use the visit_layers_range to modify only a specific set of
    // layers:
    visit_layers_range<0, 2>(net, visitor_weight_decay_multiplier(1));

    // Sometimes we might want to set the learning rate differently thoughout the network.
    // Here we show how to use adjust the learning rate at the different ResNet50's
    // convolutional blocks:
    set_learning_rate_multipliers_range<  0,   2>(net, 1);
    set_learning_rate_multipliers_range<  2,  38>(net, 0.1);
    set_learning_rate_multipliers_range< 38, 107>(net, 0.01);
    set_learning_rate_multipliers_range<107, 154>(net, 0.001);
    set_learning_rate_multipliers_range<154, 193>(net, 0.0001);

    // Finally, we can check the results by printing the network.  But before, if we
    // forward an image through the network, we will see tensors shape at every layer.
    matrix<rgb_pixel> image(224, 224);
    assign_all_pixels(image, rgb_pixel(0, 0, 0));
    std::vector<matrix<rgb_pixel>> minibatch(1, image);
    resizable_tensor input;
    net.to_tensor(minibatch.begin(), minibatch.end(), input);
    net.subnet().forward(input);
    cout << net << endl;
    cout << "input size=(" <<
       "num:" << input.num_samples() << ", " <<
       "k:" << input.k() << ", " <<
       "nr:" << input.nr() << ", "
       "nc:" << input.nc() << ")" << endl;

    // We can also print the number of parameters of the network:
    cout << "number of network parameters: " << count_parameters(net) << endl;

    // From this point on, we can finetune the new network using this pretrained backbone
    // on another task, such as the one showed in dnn_metric_learning_on_images_ex.cpp.

    return EXIT_SUCCESS;
}
catch (const serialization_error& e)
{
    cout << e.what() << endl;
    cout << "You need to download a copy of the file resnet50_1000_imagenet_classifier.dnn" << endl;
    cout << "available at http://dlib.net/files/resnet50_1000_imagenet_classifier.dnn.bz2" << endl;
    cout << endl;
    return EXIT_FAILURE;
}
catch (const exception& e)
{
    cout << e.what() << endl;
    return EXIT_FAILURE;
}


#include "dependent_hierarchy.h"

#include <Eigen/Dense>
#include <cassert>
#include <memory>
#include <random>
#include <stan/math/prim.hpp>

void DependentHierarchy::add_datum(const int id, const Eigen::VectorXd &datum,
                                   const Eigen::VectorXd &covariate) {
  auto it = cluster_data_idx.find(id);
  assert(it == cluster_data_idx.end());
  card += 1;
  log_card = std::log(card);
  update_summary_statistics(datum, covariate, true);
  cluster_data_idx.insert(id);
}

void DependentHierarchy::remove_datum(const int id,
                                      const Eigen::VectorXd &datum,
                                      const Eigen::VectorXd &covariate) {
  update_summary_statistics(datum, covariate, false);
  card -= 1;
  log_card = (card == 0) ? stan::math::NEGATIVE_INFTY : std::log(card);
  auto it = cluster_data_idx.find(id);
  assert(it != cluster_data_idx.end());
  cluster_data_idx.erase(it);
}

Eigen::VectorXd DependentHierarchy::like_lpdf_grid(
    const Eigen::MatrixXd &data, const Eigen::MatrixXd &covariates) const {
  Eigen::VectorXd result(data.rows());
  for (size_t i = 0; i < data.rows(); i++) {
    result(i) = like_lpdf(data.row(i), covariates.row(i));
  }
  return result;
}

Eigen::VectorXd DependentHierarchy::marg_lpdf_grid(
    const Eigen::MatrixXd &data, const Eigen::MatrixXd &covariates) const {
  Eigen::VectorXd result(data.rows());
  for (size_t i = 0; i < data.rows(); i++) {
    result(i) = marg_lpdf(data.row(i), covariates.row(i));
  }
  return result;
}


#pragma once

#include <vector>
#include <string>
#include <stdio.h>
#include <fstream>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <Eigen/Eigen>

#include "VisionConstants.hpp"
#include "utils/Cluster.hpp"
#include "types/Ipoint.hpp"

class Vocab {

public:

  //! Destructor
  ~Vocab() {};

  //! Constructor
  Vocab() {
    vec_length = 0;
  };

	int getSize(){
		return vec_length;
	}

  //! Learns a set of visual words from larger set of interest points
  void learn(std::vector<Ipoint> ipts, int num_words);

	//! Words that have non-zero index coefficients coefficients got on stop list
	void setStopWords(Eigen::VectorXf indices);

  //! Loads a set of visual words for use
  void loadVocabFile(std::string filename);

  //! Saves a set of visual words
  void saveVocabFile(std::string filename);

  //! Map a set of interest points to a sparse term frequency vector
  Eigen::VectorXf mapToVec(std::vector<Ipoint> &ipts);

	//! Map a set of interest points to a sparse term frequency vector while preserving
  // the pixel locations of the interest points
  Eigen::VectorXf mapToVec(std::vector<Ipoint> &ipts, std::vector< std::vector<float> > &pixel_location);

private:

	int vec_length;

  std::vector<Ipoint> pos_words;
  std::vector<Ipoint> neg_words;

	std::vector<Ipoint> pos_stop_words;
	std::vector<Ipoint> neg_stop_words;

};


/**
 *  @file eigen_quaternion.cpp
 *  @author Maximilian Harr <maximilian.harr@daimler.com>
 *  @date 17.06.2017
 *
 *  @brief Eigen quaternion computations
 *
 *
 *
 *          Coding Standard:
 *          wiki.ros.org/CppStyleGuide
 *          https://google.github.io/styleguide/cppguide.html
 *
 *
 *  @bug
 *
 *
 *  @todo
 *
 *
 */

// PRAGMA

// SYSTEM INCLUDES
#include <iostream>
#include <cstdlib>
#include <math.h>

#include <Eigen/Core>

#include "ros/ros.h"
#include <std_msgs/Float64MultiArray.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <std_msgs/Float64MultiArray.h>

// PROJECT INCLUDES

// LOCAL INCLUDES

// FORWARD REFERENCES

// FUNCTION PROTOTYPES

/** @brief
 *  @param
 *  @return
 */

// GLOBAL VARIABLES

using namespace Eigen;
using namespace std;

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

  /* Uncertrainty of position and estimation */
  Eigen::MatrixXd mat_position    = 1*Eigen::MatrixXd::Identity(3,3);
  Eigen::MatrixXd mat_orientation = 2*Eigen::MatrixXd::Identity(3,3);

  /* Fill uncertainty of pose (position and orientation) */
  Eigen::MatrixXd mat_pose = Eigen::MatrixXd::Zero(6,6);
  mat_pose.block(0,0,3,3) =  mat_position;
  mat_pose.block(3,3,3,3) =  mat_orientation;

  std::cout << "mat_pose:\n" << mat_pose << std::endl;

  /* @todo delete. Mapping of Eigen Matrix in std::vector and inserting in MutliArray */
  std::vector<double> covmat_std(mat_pose.data(), mat_pose.data() + mat_pose.rows() * mat_pose.cols() );
  std_msgs::Float64MultiArray array_msg;
  array_msg.data.insert( array_msg.data.end(), covmat_std.begin(), covmat_std.end() );
  std::cout << " array_msg \n " << array_msg << std::endl;

  /* Map Eigen::Matrix in PoseWithCovarianceStamped */
  geometry_msgs::PoseWithCovarianceStamped pwcs;
  pwcs.pose.covariance[0] = 0.0;
  Eigen::VectorXd::Map( &pwcs.pose.covariance[0], mat_pose.size() ) = mat_pose;

  return 0;

}

//// FUNCTION DEFINITIONS //////////////////////////////////////////////////////////////////////////


//==============================================================================
//         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
//==============================================================================
#include <nt2/core/utility/config.hpp>
#include <nt2/core/utility/lagged_fibonacci2281stream.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/normal_distribution.hpp>

namespace nt2
{
  void lagged_fibonacci2281stream_::seed(uint32_t s)
  {
    generator_.seed(s);
  }

  void lagged_fibonacci2281stream_::randn(double* data, std::size_t i0, std::size_t i1)
  {
    boost::random::normal_distribution<double> dist;
    for(std::size_t i=i0;i<i1;++i) data[i] = dist(generator_);
  }

  void lagged_fibonacci2281stream_::randn(float*  data, std::size_t i0, std::size_t i1)
  {
    boost::random::normal_distribution<float> dist;
    for(std::size_t i=i0;i<i1;++i) data[i] = dist(generator_);
  }

  void lagged_fibonacci2281stream_::rand(double* data, std::size_t i0, std::size_t i1)
  {
    boost::random::uniform_real_distribution<double> dist;
    for(std::size_t i=i0;i<i1;++i) data[i] = dist(generator_);
  }

  void lagged_fibonacci2281stream_::rand(float*  data, std::size_t i0, std::size_t i1)
  {
    boost::random::uniform_real_distribution<float> dist;
    for(std::size_t i=i0;i<i1;++i) data[i] = dist(generator_);
  }

  lagged_fibonacci2281stream_::~lagged_fibonacci2281stream_()
  {
  }

  nt2::randstream_* lagged_fibonacci2281stream()
  {
    static nt2::lagged_fibonacci2281stream_ instance;
    return &instance;
  }
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/range/aligned_range.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <boost/simd/literal.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/function/splat.hpp>
#include <boost/range/algorithm/generate.hpp>
#include <simd_test.hpp>
#include <vector>

using namespace boost::simd::literal;

STF_CASE_TPL("distance", STF_NUMERIC_TYPES)
{
  using boost::simd::aligned_range;
  using boost::simd::pack;

  std::vector<T,boost::simd::allocator<T>> data(pack<T>::static_size*3);
  std::vector<T,boost::simd::allocator<T,8>> data2(24);

  auto rng  = aligned_range(data);
  auto rng8 = aligned_range(data2, 8_c);

  STF_EQUAL( std::distance(boost::begin(rng) , boost::end(rng) ), 3 );
  STF_EQUAL( std::distance(boost::begin(rng8), boost::end(rng8)), 3 );
}

template<class T> struct generate
{
  generate(T v) : value(v) {}
  boost::simd::pack<T> operator()() const { return boost::simd::pack<T>(value++); }

  private:
  mutable T value;
};

STF_CASE_TPL("iteration", STF_NUMERIC_TYPES)
{
  using boost::simd::aligned_range;
  using boost::simd::pack;

  std::size_t ps = pack<T>::static_size;
  std::vector<T, boost::simd::allocator<T> >  ref (4*ps), data(4*ps);

  for(std::size_t i=0;i<data.size();i++)  data[i] = T(0);
  for(std::size_t i=0;i<ref.size();i++)   ref[i]  = T(i/pack<T>::static_size+1);

  auto simd_range = aligned_range(data);

  T k = 0;
  for(auto&& e : simd_range) e = pack<T>(++k);

  STF_EQUAL( ref, data );
}


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

#pragma once

#include <Eigen/Core>

#include "ngraph/shape.hpp"
#include "ngraph/strides.hpp"

namespace ngraph
{
    namespace runtime
    {
        namespace cpu
        {
            namespace eigen
            {
                using DynamicStrides = Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>;
                using VectorStrides = Eigen::Stride<Eigen::Dynamic, 1>;

                template <typename T>
                using DynamicArray =
                    Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

                template <typename T>
                using EigenArrayBase = Eigen::Map<DynamicArray<T>, 0, DynamicStrides>;

                template <typename T>
                using DynamicMatrix =
                    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

                template <typename T>
                using EigenMatrixBase = Eigen::Map<DynamicMatrix<T>, 0, DynamicStrides>;

                template <typename T>
                using DynamicVector = Eigen::Matrix<T, Eigen::Dynamic, 1>;

                template <typename T>
                using EigenVectorBase = Eigen::Map<DynamicVector<T>, 0, VectorStrides>;

                namespace fmt
                {
                    /// \brief vector format for Eigen wrappers.
                    class V
                    {
                    public:
                        V(size_t s)
                            : l0(s)
                        {
                        }

                    public:
                        size_t l0;
                        size_t l1{1};
                        size_t s0{1};
                        size_t s1{1};
                    };

                    class M
                    {
                    public:
                        M(const Shape& shape, const Strides& strides)
                            : l0(shape.at(0))
                            , l1(shape.at(1))
                            , s0(strides.at(0))
                            , s1(strides.at(1))
                        {
                        }

                    public:
                        size_t l0;
                        size_t l1;
                        size_t s0;
                        size_t s1;
                    };
                }

                // T element type
                // FMT array format (fmt::V for vector, etc.)
                // BASE select array/matrix
                template <typename T,
                          typename FMT,
                          typename BASE,
                          typename STRIDES = DynamicStrides>
                class EigenWrapper : public BASE
                {
                    using base = BASE;

                public:
                    EigenWrapper(T* t, const FMT& fmt)
                        : base(t, fmt.l0, fmt.l1, STRIDES(fmt.s0, fmt.s1))
                    {
                    }

                    template <typename U>
                    EigenWrapper& operator=(const U& other)
                    {
                        this->base::operator=(other);
                        return *this;
                    }
                };

                template <typename T, typename FMT = fmt::V>
                using EigenArray1d = EigenWrapper<T, FMT, EigenArrayBase<T>>;

                template <typename T, typename FMT = fmt::M>
                using EigenArray2d = EigenWrapper<T, FMT, EigenArrayBase<T>>;

                template <typename T, typename FMT = fmt::M>
                using EigenMatrix = EigenWrapper<T, FMT, EigenMatrixBase<T>>;

                template <typename T, typename FMT = fmt::V>
                using EigenVector = EigenWrapper<T, FMT, EigenVectorBase<T>, VectorStrides>;
            }
        }
    }
}


//=======================================================================
// Copyright (C) 2012 Flavio De Lorenzi (fdlorenzi@gmail.com)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/vf2_sub_graph_iso.hpp>
using namespace boost;

int main()
{
    typedef property< edge_name_t, char > edge_property;
    typedef property< vertex_name_t, char, property< vertex_index_t, int > >
        vertex_property;

    // Using a vecS graphs => the index maps are implicit.
    typedef adjacency_list< vecS, vecS, bidirectionalS, vertex_property,
        edge_property >
        graph_type;

    // Build graph1
    graph_type graph1;

    add_vertex(vertex_property('a'), graph1);
    add_vertex(vertex_property('a'), graph1);
    add_vertex(vertex_property('a'), graph1);

    add_edge(0, 1, edge_property('b'), graph1);
    add_edge(0, 1, edge_property('b'), graph1);
    add_edge(0, 1, edge_property('d'), graph1);

    add_edge(1, 2, edge_property('s'), graph1);

    add_edge(2, 2, edge_property('l'), graph1);
    add_edge(2, 2, edge_property('l'), graph1);

    // Build graph2
    graph_type graph2;

    add_vertex(vertex_property('a'), graph2);
    add_vertex(vertex_property('a'), graph2);
    add_vertex(vertex_property('a'), graph2);
    add_vertex(vertex_property('a'), graph2);
    add_vertex(vertex_property('a'), graph2);
    add_vertex(vertex_property('a'), graph2);

    add_edge(0, 1, edge_property('a'), graph2);
    add_edge(0, 1, edge_property('a'), graph2);
    add_edge(0, 1, edge_property('b'), graph2);

    add_edge(1, 2, edge_property('s'), graph2);

    add_edge(2, 3, edge_property('b'), graph2);
    add_edge(2, 3, edge_property('d'), graph2);
    add_edge(2, 3, edge_property('b'), graph2);

    add_edge(3, 4, edge_property('s'), graph2);

    add_edge(4, 4, edge_property('l'), graph2);
    add_edge(4, 4, edge_property('l'), graph2);

    add_edge(4, 5, edge_property('c'), graph2);
    add_edge(4, 5, edge_property('c'), graph2);
    add_edge(4, 5, edge_property('c'), graph2);

    add_edge(5, 0, edge_property('s'), graph2);

    // create predicates
    typedef property_map< graph_type, vertex_name_t >::type vertex_name_map_t;
    typedef property_map_equivalent< vertex_name_map_t, vertex_name_map_t >
        vertex_comp_t;
    vertex_comp_t vertex_comp = make_property_map_equivalent(
        get(vertex_name, graph1), get(vertex_name, graph2));

    typedef property_map< graph_type, edge_name_t >::type edge_name_map_t;
    typedef property_map_equivalent< edge_name_map_t, edge_name_map_t >
        edge_comp_t;
    edge_comp_t edge_comp = make_property_map_equivalent(
        get(edge_name, graph1), get(edge_name, graph2));

    // Create callback
    vf2_print_callback< graph_type, graph_type > callback(graph1, graph2);

    // Print out all subgraph isomorphism mappings between graph1 and graph2.
    // Function vertex_order_by_mult is used to compute the order of
    // vertices of graph1. This is the order in which the vertices are examined
    // during the matching process.
    vf2_subgraph_iso(graph1, graph2, callback, vertex_order_by_mult(graph1),
        edges_equivalent(edge_comp).vertices_equivalent(vertex_comp));

    return 0;
}


#ifndef BLUB_MATH_HPP
#define BLUB_MATH_HPP

#include "blub/core/globals.hpp"

#include <boost/config/suffix.hpp>

#include <algorithm>

namespace blub
{

class math
{
public:

#if defined(BOOST_NO_CXX11_CONSTEXPR)
    static const real math::pi;
    static const real math::piHalf;
    static const real math::sqrtZeroPointFive;
    static const real math::sqrtOneThird;
#else
    static constexpr real pi = 3.14159265358979323846;
    static constexpr real piHalf = pi / 2.;
    static constexpr real sqrtZeroPointFive = 0.707106781;
    static constexpr real sqrtOneThird = 0.577350269; // sqrt(1/3)
#endif

    template <typename T>
    static T min(const T &val, const T &minimum)
    {
        return std::min(val, minimum);
    }
    template <typename T>
    static T max(const T &val, const T &maximum)
    {
        return std::max(val, maximum);
    }
    static real abs(const real& calc);

    template <typename T>
    static T clamp(const T& val, const T& minval, const T& maxval)
    {
        BASSERT(minval <= maxval);
        return std::max(std::min(val, maxval), minval);
    }
    template <typename T, typename pairType>
    static T clamp(const T& val, const pairType& minMaxVal)
    {
        BASSERT(minMaxVal.first <= minMaxVal.second);
        return clamp(val, minMaxVal.first, minMaxVal.second);
    }

    static real map(const real &value, const real &fromMin, const real &fromMax, const real &toMin, const real &toMax);

    template <typename T>
    static T floor(const T& calc)
    {
        return std::floor(calc);
    }
    template <typename T>
    static T ceil(const T& val)
    {
        return std::ceil(val);
    }

    template <typename T>
    static bool between(const T& number, const T& min, const T& max)
    {
        return (number >= min) && (number <= max);
    }

    static real sin(const real& calc);
    static real asin(const real& calc);
    static real cos(const real& calc);
    static real acos(const real& calc);
    static real atan2(const real& fst, const real& snd);

    static real sqrt(const real& calc);

    static int32 pow(const int32& number, const int32& expo);
    static real pow(const real& number, const real& expo);

    static uint32 rand();
    static real randReal();

private:
    math()
    {;}

};

}

#endif // BLUB_MATH_HPP


// Copyright 2005 The Trustees of Indiana University.

// Use, modification and distribution is subject to 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)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#include <boost/graph/topological_sort.hpp>
#include "graph_types.hpp"
#include <boost/python.hpp>
#include <boost/graph/python/list_append_iterator.hpp>

namespace boost { namespace graph { namespace python {

using boost::python::list;

template<typename Graph>
list
topological_sort
  (const Graph& g,
   vector_property_map<
     default_color_type,
     typename property_map<Graph, vertex_index_t>::const_type>* in_color)
{
  typedef vector_property_map<
            default_color_type,
            typename property_map<Graph, vertex_index_t>::const_type> ColorMap;

  ColorMap color =
    in_color? *in_color : ColorMap(num_vertices(g), get(vertex_index, g));

  list result;
  boost::topological_sort(g, list_append_iterator(result), color_map(color));
  return result;
}

void export_topological_sort()
{
  using boost::python::arg;
  using boost::python::def;

#define UNDIRECTED_GRAPH(Name,Type)                                     \
  {                                                                     \
    typedef vector_property_map<                                        \
              default_color_type,                                       \
              property_map<Graph, vertex_index_t>::const_type> ColorMap; \
    def("topological_sort", &topological_sort<Type>,                    \
        (arg("graph"), arg("color_map") = static_cast<ColorMap*>(0)));  \
  }
#include "graphs.hpp"
}

} } } // end namespace boost::graph::python


///////////////////////////////////////////////////////////////////////////////
// statistics::detail::accumulator::tag::threshold.hpp                       //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_ACCUMULATOR_STATISTICS_TAG_THRESHOLD_HPP_2009
#define BOOST_STATISTICS_DETAIL_ACCUMULATOR_STATISTICS_TAG_THRESHOLD_HPP_2009
#include <boost/parameter/name.hpp>
#include <boost/parameter/keyword.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace accumulator{
namespace tag{
    struct threshold{};
}// tag
}// accumulator
}// detail
}// statistics
}// boost

#endif


/*
 [auto_generated]
 libs/numeric/odeint/test/multi_array.cpp

 [begin_description]
 tba.
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_multi_array

#include <boost/test/unit_test.hpp>

#include <boost/numeric/odeint/algebra/multi_array_algebra.hpp>
#include <boost/numeric/odeint/util/multi_array_adaption.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/integrate/integrate_const.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include <boost/numeric/odeint/stepper/generation.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

typedef boost::multi_array< double , 1 > vector_type;
typedef boost::multi_array< double , 2 > matrix_type;
typedef boost::multi_array< double , 3 > tensor_type;

BOOST_AUTO_TEST_SUITE( multi_array_test )

BOOST_AUTO_TEST_CASE( test_resizeable )
{
    BOOST_CHECK( is_resizeable< vector_type >::value );
    BOOST_CHECK( is_resizeable< matrix_type >::value );
    BOOST_CHECK( is_resizeable< tensor_type >::value );
}

BOOST_AUTO_TEST_CASE( test_same_size_vector1 )
{
    vector_type v1( boost::extents[12] );
    vector_type v2( boost::extents[12] );
    BOOST_CHECK( same_size( v1 , v2 ) );
}

BOOST_AUTO_TEST_CASE( test_same_size_vector2 )
{
    vector_type v1( boost::extents[12] );
    vector_type v2( boost::extents[13] );
    BOOST_CHECK( !same_size( v1 , v2 ) );
}

BOOST_AUTO_TEST_CASE( test_same_size_vector3 )
{
    vector_type v1( boost::extents[12] );
    vector_type v2( boost::extents[vector_type::extent_range(-1,11)] );
    BOOST_CHECK( !same_size( v1 , v2 ) );
}

BOOST_AUTO_TEST_CASE( test_same_size_vector4 )
{
    vector_type v1( boost::extents[12] );
    vector_type v2( boost::extents[vector_type::extent_range(-1,10)] );
    BOOST_CHECK( !same_size( v1 , v2 ) );
}

BOOST_AUTO_TEST_CASE( test_same_size_vector5 )
{
    vector_type v1( boost::extents[vector_type::extent_range(-1,10)] );
    vector_type v2( boost::extents[vector_type::extent_range(-1,10)] );
    BOOST_CHECK( same_size( v1 , v2 ) );
}

BOOST_AUTO_TEST_CASE( test_same_size_matrix1 )
{
    matrix_type m1( boost::extents[12][4] );
    matrix_type m2( boost::extents[12][4] );
    BOOST_CHECK( same_size( m1 , m2 ) );
}

BOOST_AUTO_TEST_CASE( test_same_size_matrix2 )
{
    matrix_type m1( boost::extents[12][4] );
    matrix_type m2( boost::extents[12][3] );
    BOOST_CHECK( !same_size( m1 , m2 ) );
}

BOOST_AUTO_TEST_CASE( test_same_size_matrix3 )
{
    matrix_type m1( boost::extents[12][matrix_type::extent_range(-1,2)] );
    matrix_type m2( boost::extents[12][3] );
    BOOST_CHECK( !same_size( m1 , m2 ) );
}

BOOST_AUTO_TEST_CASE( test_same_size_matrix4 )
{
    matrix_type m1( boost::extents[12][matrix_type::extent_range(-1,1)] );
    matrix_type m2( boost::extents[12][3] );
    BOOST_CHECK( !same_size( m1 , m2 ) );
}

BOOST_AUTO_TEST_CASE( test_same_size_tensor1 )
{
    tensor_type t1( boost::extents[ tensor_type::extent_range( -2 , 9 ) ]
                                  [ tensor_type::extent_range( 5 , 11 ) ]
                                  [ tensor_type::extent_range( -1 , 2 ) ] );
    tensor_type t2( boost::extents[ tensor_type::extent_range( 2 , 13 ) ]
                                  [ tensor_type::extent_range( -1 , 5 ) ]
                                  [ tensor_type::extent_range( 1 , 4 ) ] );
    BOOST_CHECK( !same_size( t1 , t2 ) );
}

BOOST_AUTO_TEST_CASE( test_same_size_tensor2 )
{
    tensor_type t1( boost::extents[ tensor_type::extent_range( -2 , 9 ) ]
                                  [ tensor_type::extent_range( 5 , 11 ) ]
                                  [ tensor_type::extent_range( -1 , 2 ) ] );
    tensor_type t2( boost::extents[ tensor_type::extent_range( -2 , 9 ) ]
                                  [ tensor_type::extent_range( 5 , 11 ) ]
                                  [ tensor_type::extent_range( -1 , 2 ) ] );
    BOOST_CHECK( same_size( t1 , t2 ) );
}

// Tests for tensor_type

BOOST_AUTO_TEST_CASE( test_resize_vector1 )
{
    vector_type v1( boost::extents[4] );
    vector_type v2;
    resize( v2 , v1 );
    BOOST_CHECK( same_size( v1 , v2 ) );
}

BOOST_AUTO_TEST_CASE( test_resize_vector2 )
{
    vector_type v1( boost::extents[ vector_type::extent_range( -2 , 9 ) ] );
    vector_type v2;
    resize( v2 , v1 );
    BOOST_CHECK( same_size( v1 , v2 ) );
}

BOOST_AUTO_TEST_CASE( test_resize_vector3 )
{
    vector_type v1( boost::extents[ vector_type::extent_range( -2 , 9 ) ] );
    vector_type v2( boost::extents[ vector_type::extent_range( 2 , 9 ) ] );
    BOOST_CHECK( !same_size( v1 , v2 ) );
    resize( v2 , v1 );
    BOOST_CHECK( same_size( v1 , v2 ) );
}

struct mult4
{
    void operator()( double &a , double const &b ) const
    {
        a = b * 4.0;
    }
};

BOOST_AUTO_TEST_CASE( test_for_each2_vector )
{
    vector_type v1( boost::extents[ vector_type::extent_range( -2 , 9 ) ] );
    vector_type v2( boost::extents[ vector_type::extent_range( 2 , 13 ) ] );
    for( int i=-2 ; i<9 ; ++i )
        v1[i] = i * 2;
    multi_array_algebra::for_each2( v2 , v1 , mult4() );
    for( int i=2 ; i<13 ; ++i )
        BOOST_CHECK_EQUAL( v2[i] , v1[i-4]*4.0 );
}

struct vector_ode
{
    const static size_t n = 128;
    void operator()( const vector_type &x , vector_type &dxdt , double t ) const
    {
        dxdt[-1] = x[n] - 2.0 * x[-1] + x[0];
        for( size_t i=0 ; i<n ; ++i )
            dxdt[i] = x[i-1] - 2.0 * x[i] + x[i+1];
        dxdt[n] = x[-1] - 2.0 * x[n] + x[n-1];
    }
};

BOOST_AUTO_TEST_CASE( test_rk4_vector )
{
    vector_type v1( boost::extents[ vector_type::extent_range( -1 , vector_ode::n+1 ) ] );
    integrate_const( runge_kutta4< vector_type >() , vector_ode() , v1 , 0.0 , 1.0 , 0.01 );
}

BOOST_AUTO_TEST_CASE( test_dopri5_vector )
{
    vector_type v1( boost::extents[ vector_type::extent_range( -1 , vector_ode::n+1 ) ] );
    integrate_const( make_dense_output( 1.0e-6 , 1.0e-6 , runge_kutta_dopri5< vector_type >() ) , vector_ode() , v1 , 0.0 , 1.0 , 0.01 );
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/integral_constant.hpp>
namespace hana = boost::hana;
using namespace hana::literals;

static constexpr auto x = 4'321'000_c;
static_assert(decltype(x)::value == 4'321'000, "");

int main() { }


#include <stdio.h>
#include <string>
#include <vector>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>

#ifdef _OPENMP
#include <omp.h>
#endif

#include <boost/dynamic_bitset.hpp>

SparseDist pairgene(std::string rtab, int n_threads, int dist, int knn)
{
    // first get number of genes
    int ngenes = 0;
    std::string line;
    std::ifstream gf(rtab);

    while (std::getline(gf, line))
        ++ngenes;
    ngenes -= 1;

    // open filename
    size_t n_seqs = 0;

    // initialise bitmaps
    std::vector<std::string> seq_names;
    std::vector<boost::dynamic_bitset<>> allgenes;

    SparseDist results;

    gf.clear();
    gf.seekg(0);

    int linenum=0;
    while (getline(gf, line))
    {
        std::istringstream iss(line);
        std::string token;
        std::vector<std::string> parts;
        while (std::getline(iss, token, '\t'))
        { // but we can specify a different one
            parts.push_back(token);
        }
        if (linenum == 0)
        {
            n_seqs = parts.size() - 1;
            for (size_t j = 0; j < n_seqs; j++)
            {
                boost::dynamic_bitset<> genes(ngenes);
                allgenes.push_back(genes);
                results.seq_names.push_back(parts[j+1]);
            }
        }
        else
        {

            for (size_t j = 0; j < n_seqs; j++)
            {
                char character = parts[j+1][0];
                if (character == '1')
                {
                    allgenes[j][linenum-1] = 1;
                }
                else
                {
                    allgenes[j][linenum-1] = 0;
                }
            }
        }
        linenum+=1;
    }
    gf.close();

#pragma omp parallel for ordered shared(allgenes, ngenes, n_seqs, seq_names, dist, knn, results) default(none) schedule(static, 1) num_threads(n_threads)
    for (size_t i = 0; i < n_seqs; i++)
    {

        std::vector<int> comp_snps(n_seqs);
        boost::dynamic_bitset<> res(ngenes);

        size_t start = 0;
        if (knn < 0)
        {
            start = i + 1;
        }
        else
        {
            start = 0;
        }

        for (size_t j = start; j < n_seqs; j++)
        {
            res = allgenes[i] & allgenes[j];
            comp_snps[j] = ngenes - res.count();
        }

        // if using knn find the distance needed
        if (knn >= 0)
        {
            std::vector<int> s_comp = comp_snps;
            std::sort(s_comp.begin(), s_comp.end());
            dist = s_comp[knn + 1];
            start = 0;
        }
        else
        {
            start = i + 1;
        }

// output distances
#pragma omp critical
        for (size_t j = 0; j < n_seqs; j++)
        {
            if ((dist == -1) || (comp_snps[j] <= dist))
            {
                results.rows.push_back(i);
                results.cols.push_back(j);
                results.distances.push_back(comp_snps[j]);
            }
        }

        if (i % 10 == 0)
        {
            fprintf(stdout, "Calculating distances: %.1lf%%\n", (double)i / n_seqs * 100);
            fflush(stdout);
        }
    }

    return results;
}


// [===========================================================================]
// [                                M o n e t a                                ]
// [---------------------------------------------------------------------------]
// [                                                                           ]
// [                          Copyright (C) 2005-2015                          ]
// [                      Rodrigo Madera <madera@acm.org>                      ]
// [                                                                           ]
// [---------------------------------------------------------------------------]
// [         Distributed under the Boost Software License, Version 1.0         ]
// [ Read accompanying LICENSE or copy at http://www.boost.org/LICENSE_1_0.txt ]
// [===========================================================================]

#pragma once
#include <boost/mpl/clear.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/copy.hpp>

namespace mplx {

	template<class Sequence>
	struct flatten : boost::mpl::fold<
		Sequence,
		typename boost::mpl::clear<Sequence>::type,
		boost::mpl::if_<
			boost::mpl::is_sequence<boost::mpl::_2>,
			boost::mpl::copy<
				mplx::flatten<boost::mpl::_2>,
				boost::mpl::back_inserter<boost::mpl::_1>
			>,
			boost::mpl::push_back<boost::mpl::_1, boost::mpl::_2>
		>
	> {};

}


//
// boost-geometry-render
//
// Copyright (c) 2011
// osyo-manga : http://d.hatena.ne.jp/osyo-manga/
//
// License:
// Boost Software License - Version 1.0
// <http://www.boost.org/LICENSE_1_0.txt>
//
#include <cstdlib>
#include <gl/glut.h>
#include <gl/graphics.hpp>
#include <gl/geometry_render.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/thread.hpp>
#include <boost/geometry/algorithms/intersects.hpp>

template<typename Geometry, typename Transformer>
void
transform(Geometry& geometry, Transformer const& transfomer){
	Geometry tmp;
	boost::geometry::transform(geometry, tmp, transfomer);
	boost::geometry::assign(geometry, tmp);
}

int
main(int argc, char* argv[]){
	namespace bg = boost::geometry;

	typedef bg::model::d2::point_xy<float> point_type;

	bg::model::box<point_type> b1(point_type(-0.5f, -0.1f), point_type(0.4f, 0.4f));
	bg::model::box<point_type> b2(point_type(-0.6f, -0.1f), point_type(0.2f, 0.3f));

	gl::graphics g(argc, argv, 500, 500);

	gl::displayfunc(g, [&](){
		glColor3d(1.0f, 0.0f, 0.0f);
		gl::render(b1);
	});

	gl::displayfunc(g, [&](){
		if( boost::geometry::intersects(b1, b2) ){
			glColor3d(1.0f, 0.0f, 0.0f);
		}
		else{
			glColor3d(0.0f, 1.0f, 0.0f);
		}
		gl::render(b2);
	});

	gl::keyboardfunc(g, [&](unsigned char key, int x, int y){
		namespace bg = boost::geometry;
		typedef bg::model::d2::point_xy<float> point_type;
		typedef bg::strategy::transform::translate_transformer<point_type, point_type> transformer;
		switch(key){
			case 'q': return exit(0);
			case 'h': return transform(b2, transformer(-0.01f, 0.0f));
			case 'j': return transform(b2, transformer(0.0f, -0.01f));
			case 'k': return transform(b2, transformer(0.0f, 0.01f));
			case 'l': return transform(b2, transformer(0.01f, 0.0f));
		}
	});

	gl::idlefunc(g, [&](){
		boost::this_thread::sleep(boost::posix_time::milliseconds(16));
		glutPostRedisplay();
	});

	g.run();
	return 0;
}


/**
 * Copyright (c) 2018, Ouster, Inc.
 * All rights reserved.
 */

#include "ouster/image_processing.h"

#include <Eigen/src/Core/util/Meta.h>

#include <Eigen/Core>
#include <Eigen/LU>
#include <algorithm>
#include <iostream>
#include <vector>

namespace ouster {
namespace viz {

namespace {

/*
 * damping makes the autoexposure smooth and avoids flickering however, it
 * becomes slower to update.
 * 1.0 --> slowest, smoothest
 * 0.0 --> fastest, prone to flickering
 */
const double ae_damping = 0.90;

/*
 * for performance reasons, we may not want to update every frame but rather
 * every few frames
 */
const int ae_default_update_every = 3;

/* for performance reasons, only consider a subset of points */
const size_t ae_stride = 4;

/* if there are too few points, do nothing */
const size_t ae_min_nonzero_points = 100;

/* default percentile for scaling in autoexposure */
const double ae_default_percentile = 0.1;

}  // namespace

AutoExposure::AutoExposure()
    : lo_percentile(ae_default_percentile),
      hi_percentile(ae_default_percentile),
      ae_update_every(ae_default_update_every) {}

AutoExposure::AutoExposure(int update_every)
    : lo_percentile(ae_default_percentile),
      hi_percentile(ae_default_percentile),
      ae_update_every(update_every) {}

AutoExposure::AutoExposure(double lo_percentile, double hi_percentile,
                           int update_every)
    : lo_percentile(lo_percentile),
      hi_percentile(hi_percentile),
      ae_update_every(update_every) {}

template <typename T>
void AutoExposure::update(Eigen::Ref<img_t<T>> image, bool update_state) {
    Eigen::Map<Eigen::Array<T, -1, 1>> key_eigen(image.data(), image.size());

    // int a;
    if (counter == 0 && update_state) {
        const size_t n = key_eigen.rows();
        std::vector<size_t> indices;
        indices.reserve(n);
        for (size_t i = 0; i < n; i += ae_stride) {
            // ignore 0 values, which are often due to dropped packets etc
            if (key_eigen[i] > 0) {
                indices.push_back(i);
            }
        }
        if (indices.size() < ae_min_nonzero_points) {
            // too few nonzero values, nothing to do
            return;
        }
        auto cmp = [&](const size_t a, const size_t b) {
            return key_eigen(a) < key_eigen(b);
        };

        const size_t lo_kth_extreme = indices.size() * lo_percentile;
        std::nth_element(indices.begin(), indices.begin() + lo_kth_extreme,
                         indices.end(), cmp);
        lo = key_eigen[*(indices.begin() + lo_kth_extreme)];

        const size_t hi_kth_extreme = indices.size() * hi_percentile;
        std::nth_element(indices.begin() + lo_kth_extreme,
                         indices.end() - hi_kth_extreme - 1, indices.end(),
                         cmp);
        hi = key_eigen[*(indices.end() - hi_kth_extreme - 1)];

        if (!initialized) {
            initialized = true;
            lo_state = lo;
            hi_state = hi;
        }
    }
    if (!initialized) {
        return;
    }

    // we use the simplest form of exponential smoothing
    if (update_state) {
        lo_state = ae_damping * lo_state + (1.0 - ae_damping) * lo;
        hi_state = ae_damping * hi_state + (1.0 - ae_damping) * hi;
    }

    // Apply affine transformation mapping lo_state to lo_percentile and
    // hi_state to 1 - hi_percentile. If it would map 0 to positive number,
    // instead map using only hi_state
    double lo_hi_scale =
        (1.0 - (lo_percentile + hi_percentile)) / (hi_state - lo_state);

    if (std::isinf(lo_hi_scale) || std::isnan(lo_hi_scale)) {
        // map everything relative to hi_state being 0.5 due to small spread or
        // nan
        key_eigen *= 0.5 / hi_state;
    } else if (lo_hi_scale * (0.0 - lo_state) + lo_percentile <= 0.00) {
        // apply affine transformation
        key_eigen -= lo_state;
        key_eigen *= lo_hi_scale;
        key_eigen += lo_percentile;
    } else {
        // lo_hi_state transformation would map 0 to positive number
        // instead, map using only hi_state
        key_eigen *= (1.0 - hi_percentile) / (hi_state);
    }

    // clamp
    key_eigen = key_eigen.max(0.0).min(1.0);

    if (update_state) {
        counter = (counter + 1) % ae_update_every;
    }
}

// use overloads vs templates so implicit conversion to Eigen::Ref still works
void AutoExposure::operator()(Eigen::Ref<img_t<float>> image,
                              bool update_state) {
    update(image, update_state);
}

void AutoExposure::operator()(Eigen::Ref<img_t<double>> image,
                              bool update_state) {
    update(image, update_state);
}

namespace {

/*
 * damping makes the correction smooth and avoids flickering.
 * however, it becomes slower to update.
 * 1.0 --> slowest, smoothest
 * 0.0 --> fastest, prone to flickering
 */
const double buc_damping = 0.92;

/*
 * for performance reasons, we may not want to update every frame
 * but rather every 8 or so frames.
 */
const int buc_update_every = 8;

}  // namespace

/*
 * computes the dark count, i.e. an additive offset in the brightness of the
 * image, to smoothe the difference between rows
 */
template <typename T>
static Eigen::Array<T, -1, 1> compute_dark_count(
    const Eigen::Ref<img_t<T>>& image) {
    const size_t image_h = image.rows();
    const size_t image_w = image.cols();

    Eigen::Array<T, -1, 1> tmp{image_w};
    Eigen::Array<T, -1, 1> new_dark_count{image_h};

    // probably computed lazily when used below?
    auto row_diffs = image.bottomRows(image_h - 1) - image.topRows(image_h - 1);

    // to handle azimuth-masked data, only consider columns with nonzero values
    Eigen::Array<bool, -1, 1> col_mask =
        image.template cast<bool>().colwise().any();
    const size_t n_cols = col_mask.count();

    img_t<T> row_diffs_nonzero{image_h - 1, n_cols};
    for (size_t i = 0, j = 0; i < image_w && j < n_cols; i++) {
        if (col_mask[i]) {
            row_diffs_nonzero.col(j) = row_diffs.col(i);
            j++;
        }
    }

    // compute the median of differences between rows
    for (size_t i = 1; i < image_h; i++) {
        tmp = row_diffs_nonzero.row(i - 1);
        std::nth_element(tmp.data(), tmp.data() + n_cols / 2,
                         tmp.data() + n_cols);
        new_dark_count[i] = new_dark_count[i - 1] + tmp[n_cols / 2];
    }

    // remove gradients in the entire height of image by doing linear fit
    Eigen::Matrix<T, -1, 2> A(image_h, 2);
    for (size_t i = 0; i < image_h; i++) {
        A(i, 0) = 1;
        A(i, 1) = i;
    }
    Eigen::Matrix<T, 2, 1> x = A.fullPivLu().solve(new_dark_count.matrix());
    new_dark_count -= (A * x).array();

    // subtract minimum value
    new_dark_count -= new_dark_count.minCoeff();
    return new_dark_count;
}

template <typename T>
void BeamUniformityCorrector::update(Eigen::Ref<img_t<T>> image,
                                     bool update_state) {
    const auto image_h = image.rows();

    // compute dark counts, if necessary
    if (dark_count.size() != image_h) {
        dark_count = compute_dark_count(image).template cast<double>();
    } else if (update_state && counter == 0) {
        // if previous state exists, update using exponential smoothing:
        Eigen::ArrayXd new_dark_count =
            compute_dark_count(image).template cast<double>();
        dark_count *= buc_damping;
        dark_count += new_dark_count * (1.0 - buc_damping);
    }
    counter = (counter + 1) % buc_update_every;

    // apply the dark count correction
    image.colwise() -= dark_count.cast<T>();

    // clamp any negative values
    image = image.cwiseMax(0.0);
}

void BeamUniformityCorrector::operator()(Eigen::Ref<img_t<float>> image,
                                         bool update_state) {
    update(image, update_state);
}

void BeamUniformityCorrector::operator()(Eigen::Ref<img_t<double>> image,
                                         bool update_state) {
    update(image, update_state);
}

}  // namespace viz
}  // namespace ouster