/* ----------------------------------------------------------------------- *//**
 *
 * @file svd.cpp
 *
 * @brief Functions for Singular Value Decomposition
 *
 * @date Jul 10, 2013
 *//* ----------------------------------------------------------------------- */

#include <dbconnector/dbconnector.hpp>
#include <math.h>
#include <iostream>
#include <algorithm>
#include <functional>
#include <numeric>
#include "svd.hpp"
#include <Eigen/SVD>

namespace madlib {

using namespace dbal::eigen_integration;// Use Eigen

namespace modules {
namespace linalg {

using madlib::dbconnector::postgres::madlib_construct_array;

// To get a rank-k approximation of the original matrix if we perform k + s Lanczos
// bidiagonalization steps followed by the SVD of a small matrix B(k+s) then the
// algorithm constructs the best rank-k subspace in an extended subspace
// Span[U(k+s)]. Hence we obtain a better rank-k approximation than the one
// obtained after k steps steps of the standard Lanczos bidiagonalization algorithm.
// There is a memory limit to the number of extended steps and we restrict that to
// fixed number of steps for now.
// Magic number computed using the 1GB memory limit.
// MAX_LANCZOS_STEPS^2 < 10^9 bytes / (8 bytes * 3 matrices)
const size_t MAX_LANCZOS_STEPS = 5000;

// For floating point equality comparisons,
// it is safer to define a small range of values
// that are "zero", rather than use the exact value of 0.
const double ZERO_THRESHOLD = 1e-8;

/* Project the vector v into the vector u (in-place) */
static void __project(MappedColumnVector& u, MutableNativeColumnVector& v){
    double uu = u.dot(u);
    double uv = u.dot(v);

    double coef;
    if (uu <= ZERO_THRESHOLD) { // if u is the zero vector, we have a division by zero problem
        coef = 0;
    } else
        coef = uv / uu;
    v = coef * u;
}

/**
 * @brief This function returns a random normalized unit vector of specified size
 * @param args[0]   The dimension
 * @return          The unit-norm vector
 **/
AnyType svd_unit_vector::run(AnyType & args)
{
    int32_t dim = args[0].getAs<int32_t>();

    if(dim < 1){
        throw std::invalid_argument(
            "invalid argument - Positive integer expected for dimension");
    }

    MutableNativeColumnVector vectorEigen;
    Allocator& allocator = defaultAllocator();
    vectorEigen.rebind(allocator.allocateArray<double>(dim));
    vectorEigen.setRandom();
    vectorEigen = vectorEigen.normalized();

    // AnyType tuple;
    // tuple << vectorEigen;
    // return tuple;

    return vectorEigen;
}

/**
 * @brief This function is the transition function of the aggregator computing the Lanczos vectors
 * @param args[0]   State variable (i.e. A * q_j OR A_trans * p_(j-1))
 * @param args[1]   Matrix row id
 * @param args[2]   Matrix row array
 * @param args[3]   Previous P/Q vector
 * @param args[4]   Row/Column dimension
 **/
AnyType svd_lanczos_sfunc::run(AnyType & args){
    int32_t row_id = args[1].getAs<int32_t>();
    MappedColumnVector row_array = args[2].getAs<MappedColumnVector >();
    MappedColumnVector vec = args[3].getAs<MappedColumnVector >();
    int32_t dim = args[4].getAs<int32_t>();

    if(dim < 1){
        throw std::invalid_argument(
            "invalid argument - Positive integer expected for dimension");
    }

    if(row_id <= 0 || row_id > dim){
        throw std::invalid_argument(
            "invalid argument: row_id is out of range [1, dim]");
    }

    if(row_array.size() != vec.size()){
        throw std::invalid_argument(
            "dimensions mismatch: row_array.size() != vec.size(). "
            "Data contains different sized arrays");
    }

    // FIXME: construct_array functions circumvent the abstraction layer. These
    // should be replaced with appropriate Allocator:: calls.
    MutableArrayHandle<double> state(NULL);
    if(args[0].isNull()){
        state = MutableArrayHandle<double>(
            madlib_construct_array(
                NULL, dim, FLOAT8OID, sizeof(double), true, 'd'));
        for (int i = 0; i < dim; i++)
            state[i] = 0;
    }else{
        state = args[0].getAs<MutableArrayHandle<double> >();
    }

    state[row_id - 1] = row_array.dot(vec);

    return state;
}

/**
 * @brief This function is the merge function of the aggregator computing the Lanczos vectors
 * @param args[0]   State variable 1
 * @param args[1]   State variable 2
 **/
AnyType svd_lanczos_prefunc::run(AnyType & args){
    MutableArrayHandle<double> state1 = args[0].getAs<MutableArrayHandle<double> >();
    ArrayHandle<double> state2 = args[1].getAs<ArrayHandle<double> >();

    if(state1.size() != state2.size()){
        throw std::runtime_error("dimension mismatch: state1.size() != state2.size()");
    }

    for(size_t i = 0; i < state1.size(); i++)
        state1[i] += state2[i];

    return state1;
}

/**
 * @breif This function completes the computation of Lanczoc P vector
 * @param args[0]   Partial P vector from the aggregator
 * @param args[1]   Previous P vector
 * @param args[2]   Previous beta
 **/
AnyType svd_lanczos_pvec::run(AnyType & args){
    MutableNativeColumnVector partial_pvec = args[0].getAs<MutableNativeColumnVector>();

    // When args[1] is NULL, it's special case for computing p_1
    if (!args[1].isNull()){
        MappedColumnVector prev_pvec = args[1].getAs< MappedColumnVector >();
        double beta = args[2].getAs<double>();

        if(partial_pvec.size() != prev_pvec.size()){
            throw std::invalid_argument(
                "dimension mismatch: partial_pvec.size() != prev_pvec.size()");
        }
        partial_pvec = partial_pvec - beta * prev_pvec;
    }

    double norm = partial_pvec.norm();
    partial_pvec.normalize();

    AnyType tuple;
    tuple << norm << partial_pvec;
    return tuple;
}

/**
 * @breif This function completes the computation of Lanczoc Q vector
 * @param args[0]   Partial Q vector from the aggregator
 * @param args[1]   Previous Q vector
 * @param args[2]   Current alpha
 **/
AnyType svd_lanczos_qvec::run(AnyType & args){
    MutableNativeColumnVector partial_qvec = args[0].getAs<MutableNativeColumnVector>();

    MappedColumnVector prev_qvec = args[1].getAs< MappedColumnVector >();
    double alpha = args[2].getAs<double>();

    if(partial_qvec.size() != prev_qvec.size()){
        throw std::invalid_argument(
            "dimension mismatch: partial_qvec.size() != prev_qvec.size()");
    }

    partial_qvec = partial_qvec - alpha * prev_qvec;

    // Different with svd_lanczos_pvec, the Q vector will be furhter orthogonalized
    // and then be normalized in a separate function
    return partial_qvec;
}

/**
 * @brief This function is the transition function of the aggregaror doing the Gram-Schmidt orthogonalization
 * @param args[0]   State variable: sum of projected vectors | vector v
 * @param args[1]   Unorthogonalized vector (v)
 * @param args[2]   Orthogonalized vector (u)
 **/
AnyType svd_gram_schmidt_orthogonalize_sfunc::run(AnyType & args){
	MutableNativeColumnVector v = args[1].getAs<MutableNativeColumnVector >();
	MappedColumnVector u = args[2].getAs<MappedColumnVector >();

    if(u.size() != v.size()){
        throw std::invalid_argument(
            "dimensions mismatch: u.size() != v.size()");
    }

    // FIXME: construct_array functions circumvent the abstraction layer. These
    // should be replaced with appropriate Allocator:: calls.
    MutableArrayHandle<double> state(NULL);
    if(args[0].isNull()){
        state = MutableArrayHandle<double>(
            madlib_construct_array(NULL,
                                   static_cast<int>(u.size()) * 2,
                                   FLOAT8OID, sizeof(double), true, 'd'));

        // Save v into the state variable
        memcpy(state.ptr() + u.size(), v.data(), v.size() * sizeof(double));
    }else{
        state = args[0].getAs<MutableArrayHandle<double> >();
    }

    // In-place projection
    __project(u, v);

    for(int i = 0; i < u.size(); i++){
        state[i] += v[i];
    }

    return state;
}

/**
 * @brief This function is the merge function of the aggregator doing the Gram-Schmidt orthogonalization
 * @param args[0]   State variable 1
 * @param args[1]   State variable 2
 **/
AnyType svd_gram_schmidt_orthogonalize_prefunc::run(AnyType & args){
    MutableArrayHandle<double> state1 = args[0].getAs<MutableArrayHandle<double> >();
    ArrayHandle<double> state2 = args[1].getAs<ArrayHandle<double> >();

    if(state1.size() != state2.size()){
        throw std::runtime_error("dimension mismatch: state1.size() != state2.size()");
    }

    // Note that the second half of the state variable stores the vector v
    for(size_t i = 0; i < state1.size() / 2; i++)
        state1[i] += state2[i];

    return state1;
}

/**
 * @brief This function is the final function of the aggregator doing the Gram-Schmidt orthogonalization
 * @param args[0]   State variable
 **/
AnyType svd_gram_schmidt_orthogonalize_ffunc::run(AnyType & args){
    ArrayHandle<double> state = args[0].getAs<ArrayHandle<double> >();

    MutableNativeColumnVector u;
    Allocator& allocator = defaultAllocator();
    u.rebind(allocator.allocateArray<double>(state.size() / 2));

    for(int i = 0; i < u.size(); i++){
        u[i] = state[u.size() + i] - state[i];
    }

    double norm = u.norm();
    u.normalize();

    AnyType tuple;
    tuple << norm << u;

    return tuple;
}

// -- SVD Decomposition for Bidiagonal Matrix --------------------------------
/**
 * @brief This function is the transition function of the aggregator computing the SVD
 * of a sparse bidiagonal matrix
 * @param args[0]   State variable (in-memory dense matrix)
 * @param args[1]   Dimension of the matrix (i.e. k)
 * @param args[2]   Row ID (i.e. row_id)
 * @param args[3]   Column ID (i.e. col_id)
 * @param args[4]   Value
 **/
AnyType svd_decompose_bidiagonal_sfunc::run(AnyType & args){
    if(args[1].isNull() || args[2].isNull()
        || args[3].isNull() || args[4].isNull())
        return args[0];

    int32_t k = args[1].getAs<int32_t>();
    int32_t row_id = args[2].getAs<int32_t>();
    int32_t col_id = args[3].getAs<int32_t>();
    double value = args[4].getAs<double>();

    if(k < 0){
        throw std::invalid_argument(
            "SVD error: k should be a positive integer");
    }
    if(k > (int)MAX_LANCZOS_STEPS){
        throw std::invalid_argument(
            "SVD error: k is too large, try with a value in the range of [1, 6000]");
    }
    if(row_id <= 0 || row_id > k){
        throw std::invalid_argument(
            "SVD error: row_id should be in the range of [1, k]");
    }
    if(col_id <= 0 || col_id > k){
        throw std::invalid_argument(
            "invalid parameter: col_id should be in the range of [1, k]");
    }

    // FIXME: construct_array functions circumvent the abstraction layer. These
    // should be replaced with appropriate Allocator:: calls.
    MutableArrayHandle<double> state(NULL);
    if(args[0].isNull()){
        state = MutableArrayHandle<double>(
            madlib_construct_array(NULL, k * k, FLOAT8OID, sizeof(double), true, 'd'));
    } else {
        state = args[0].getAs<MutableArrayHandle<double> >();
    }

    state[(row_id - 1) * k + col_id - 1] = value;
    return state;
}

/**
 * @brief This function is the merge function of the aggregator computing the SVD
 * of a sparse bidiagonal matrix
 * @param args[0]   State variable 1
 * @param args[1]   State varaible 2
 **/
AnyType svd_decompose_bidiagonal_prefunc::run(AnyType & args){
    MutableArrayHandle<double> state1 = args[0].getAs<MutableArrayHandle<double> >();
    ArrayHandle<double> state2 = args[1].getAs<ArrayHandle<double> >();

    if(state1.size() != state2.size()){
        throw std::runtime_error("dimension mismatch: state1.size() != state2.size()");
    }

    for(size_t i = 0; i < state1.size(); i++){
        state1[i] += state2[i];
    }

    return state1;
}

/**
 * @brief Take the final matrix and run it by Eigen JacobiSVD to get the left and right
    decompositions along with the eigen values
 **/
AnyType svd_decompose_bidiagonal_ffunc::run(AnyType & args){
    MappedColumnVector state = args[0].getAs<MappedColumnVector>();
    size_t k = static_cast<size_t>(sqrt(static_cast<double>(state.size())));

    // Note that Eigen Matrix deserializes the vector in the column order
    // Thus transpose() is needed after resize()
    Matrix b = state;
    b.resize(k, k);
    b.transposeInPlace();
    Eigen::JacobiSVD<Matrix> svd(b, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // Note that AnyType serializes the Matrix Object in the column order
    // Thus transpose() is needed before output
    Matrix u = svd.matrixU().transpose();
    Matrix v = svd.matrixV().transpose();
    Matrix s = svd.singularValues();

    AnyType tuple;
    tuple << u << v << s;
    return tuple;
}

AnyType svd_decompose_bidiag::run(AnyType & args){

    // <row_id, col_id, value> triple indicate the values of a bidiagonal matrix
    ArrayHandle<int32_t> row_id = args[0].getAs<ArrayHandle<int32_t> >();
    ArrayHandle<int32_t> col_id = args[1].getAs<ArrayHandle<int32_t> >();
    MappedColumnVector value = args[2].getAs<MappedColumnVector>();

    // since row_id, col_id start indexing from 1, the max element indicates the
    // dimension of of the bidiagonal matrix
    int32_t row_dim = *std::max_element(row_id.ptr(), row_id.ptr() + row_id.size());
    int32_t col_dim = *std::max_element(col_id.ptr(), col_id.ptr() + col_id.size());

    Matrix b = Matrix::Zero(row_dim, col_dim);
    for(size_t i = 0; i < row_id.size(); i++){
        // we use -1 since row_id and col_id start from 1
        b(row_id[i] - 1, col_id[i] - 1) = value[i];
    }

    Eigen::JacobiSVD<Matrix> svd(b, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // Note that AnyType serializes the Matrix Object in the column order
    // Thus transpose() is needed before output
    Matrix u = svd.matrixU().transpose();
    Matrix v = svd.matrixV().transpose();
    Matrix s = svd.singularValues();

    AnyType tuple;
    tuple << u << v << s;
    return tuple;
}

/**
 * @brief This function is the transition function of the aggregator computing the Lanczos vectors
 * @param args[0]   State variable (i.e. A * q_j OR A_trans * p_(j-1))
 * @param args[1]   Matrix block row id
 * @param args[2]   Matrix block col id
 * @param args[3]   Matrix block
 * @param args[4]   Previous P/Q vector
 * @param args[5]   Row/Column dimension
 **/
AnyType svd_block_lanczos_sfunc::run(AnyType & args){
    int32_t row_id = args[1].getAs<int32_t>();
    int32_t col_id = args[2].getAs<int32_t>();
    MappedMatrix block = args[3].getAs<MappedMatrix>();
    MappedColumnVector vec = args[4].getAs<MappedColumnVector >();
    int32_t dim = args[5].getAs<int32_t>();

    if(row_id <= 0){
        throw std::invalid_argument(
            "SVD error: row_id should be in the range of [1, dim]");
    }
    if(col_id <= 0){
        throw std::invalid_argument(
            "invalid parameter: col_id should be in the range of [1, dim]");
    }

    // FIXME: construct_array functions circumvent the abstraction layer. These
    // should be replaced with appropriate Allocator:: calls.
    MutableArrayHandle<double> state(NULL);
    if(args[0].isNull()){
        state = MutableArrayHandle<double>(
            madlib_construct_array(
                NULL, dim, FLOAT8OID, sizeof(double), true, 'd'));
    }else{
        state = args[0].getAs<MutableArrayHandle<double> >();
    }

    // Note that block is constructed in the column-major order
    size_t row_size = block.cols();
    size_t col_size = block.rows();

    Matrix v = block.transpose() * vec.segment((col_id - 1) * col_size, col_size);
    for(int32_t i = 0; i < v.rows(); i++)
        state[(row_id - 1) * row_size + i] += v.col(0)[i];

    return state;
}

/**
 * @brief This function is the transition function of the aggregator computing the Lanczos vectors
 * @param args[0]   State variable (i.e. A * q_j OR A_trans * p_(j-1))
 * @param args[1]   Row ID
 * @param args[2]   Column ID
 * @param args[3]   Value
 * @param args[4]   Previous P/Q vector
 * @param args[5]   Row/Column dimension
 **/
AnyType svd_sparse_lanczos_sfunc::run(AnyType & args){
    int32_t row_id = args[1].getAs<int32_t>();
    int32_t col_id = args[2].getAs<int32_t>();
    double value = args[3].getAs<double>();

    MappedColumnVector vec = args[4].getAs<MappedColumnVector >();
    int32_t dim = args[5].getAs<int32_t>();

    // FIXME: construct_array functions circumvent the abstraction layer. These
    // should be replaced with appropriate Allocator:: calls.
    MutableArrayHandle<double> state(NULL);
    if(args[0].isNull()){
        state = MutableArrayHandle<double>(
            madlib_construct_array(
                NULL, dim, FLOAT8OID, sizeof(double), true, 'd'));
    }else{
        state = args[0].getAs<MutableArrayHandle<double> >();
    }

    state[row_id - 1] += value * vec[col_id - 1];
    return state;
}

/*
 *  @brief In-memory multiplication of a vector with a matrix
 *  @param vec  a 1 x r vector
 *  @param mat  a r x n matrix
 *  @param k    a positive number < n
 *  @note first cut mat to r x k, then return vec * mat
 */
AnyType svd_vec_mult_matrix::run(AnyType & args){
    MappedColumnVector vec = args[0].getAs<MappedColumnVector>();
    MappedMatrix mat = args[1].getAs<MappedMatrix>();
    int32_t k = args[2].getAs<int32_t>();

    // Any integer is ok
    if(k <= 0 || k > mat.rows()){
        k = static_cast<int32_t>(mat.rows());
    }

    // Note mat is constructed in the column-first order
    // which means that mat is actually transposed
    if(vec.size() != mat.cols()){
        throw std::invalid_argument(
            "dimensions mismatch: vec.size() != matrix.rows()");
    };

    // trans(vec) * trans(mat) = mat * vec
    Matrix r = mat.topRows(k) * vec;
    ColumnVector v = r.col(0);
    return v;
}

typedef struct __sr_ctx{
    ColumnVector vec;
    Matrix mat;
    int32_t max_call;
    int32_t cur_call;
    int32_t row_id;
    int32_t k;
} sr_ctx;

/**
 * @param arg[0]    Column vector
 * @param arg[1]    Matrix (l x l)
 * @param args[2]   Column ID
 # @param arg[3]    k (Sub-matrix: l * k)
 **/
void * svd_vec_trans_mult_matrix::SRF_init(AnyType &args){
    sr_ctx * ctx = new sr_ctx;
    ctx->vec = args[0].getAs<MappedColumnVector>();
    ctx->mat = args[1].getAs<MappedMatrix>().transpose();
    ctx->row_id = args[2].getAs<int32_t>();
    ctx->k = args[3].getAs<int32_t>();

    if(ctx->row_id <= 0 || ctx->row_id > ctx->mat.rows()){
        elog(ERROR,
            "invalid parameter - row_id should be in the range of [1, mat.rows()]");
    }

    if(ctx->k > ctx->mat.cols()){
        elog(ERROR,
            "invalid parameter - k should be in the range of [0, mat.cols()]");
    }

    ctx->max_call = static_cast<int32_t>(ctx->vec.size());
    ctx->cur_call = 0;

    return ctx;
}

AnyType svd_vec_trans_mult_matrix::SRF_next(void *user_fctx, bool *is_last_call){
    sr_ctx * ctx = (sr_ctx *) user_fctx;
    if (ctx->max_call == 0) {
        *is_last_call = true;
        return Null();
    }

    ColumnVector res = ctx->vec[ctx->cur_call] *
                            ctx->mat.row(ctx->row_id - 1).segment(0, ctx->k);
    AnyType tuple;
    tuple << ctx->cur_call << res;

    ctx->cur_call++;
    ctx->max_call--;
    *is_last_call = false;

    return tuple;
}

} //namespace linalg
} // namespace modules
} //namespace madlib


#define BOOST_TEST_MODULE TemporaryTerminal
#include <boost/test/unit_test.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/multivector.hpp>
#include <vexcl/temporary.hpp>
#include <vexcl/reductor.hpp>
#include <vexcl/function.hpp>
#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(temporary)
{
    const size_t n = 1024;

    vex::vector<double> x(ctx, random_vector<double>(n));
    vex::vector<double> y(ctx, n);

    VEX_FUNCTION(double, sqr, (double, x), return x * x;);

    {
        // Deduce temporary type
        auto s = vex::make_temp<1>( sqr(x) + 25 );
        y = s * (x + s);

        check_sample(y, [&](size_t idx, double v) {
                double X = x[idx];
                double S = X * X + 25;
                BOOST_CHECK_CLOSE(v, S * (X + S), 1e-8);
                });
    }

    {
        // Provide temporary type
        auto s = vex::make_temp<1, double>( sqr(x) + 25 );
        y = s * (x + s);

        check_sample(y, [&](size_t idx, double v) {
                double X = x[idx];
                double S = X * X + 25;
                BOOST_CHECK_CLOSE(v, S * (X + S), 1e-8);
                });
    }
}

BOOST_AUTO_TEST_CASE(nested_temporary)
{
    const size_t n = 1024;

    vex::vector<double> x(ctx, random_vector<double>(n));
    vex::vector<double> y(ctx, n);

    auto t1 = vex::make_temp<1>( log(x) );
    auto t2 = vex::make_temp<2>( t1 + sin(x) );

    y = t1 * t2;

    check_sample(y, [&](size_t idx, double v) {
            double X = x[idx];
            double T1 = log(X);
            double T2 = T1 + sin(X);
            BOOST_CHECK_CLOSE(v, T1 * T2, 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(reduce_temporary)
{
    const size_t n = 1024;

    vex::vector<double> x(ctx, random_vector<double>(n));

    auto t1 = vex::make_temp<1>( pow(sin(x), 2) );
    auto t2 = vex::make_temp<2>( pow(cos(x), 2) );

    vex::Reductor<double, vex::SUM> sum(ctx);

    BOOST_CHECK_CLOSE(sum(10 * (t1 + t2)), 10.0 * n, 1e-6);
}

BOOST_AUTO_TEST_CASE(multiexpression_temporary)
{
    typedef std::array<double, 2> elem_t;
    const size_t n = 1024;

    vex::vector<double> x(ctx, random_vector<double>(n));

    vex::multivector<double, 2> y(ctx,n);

    auto tmp = vex::make_temp<1>( sin(x) );

    y = std::tie(tmp, sqrt(1 - tmp * tmp));

    check_sample(y, [&](size_t idx, elem_t v){
            double X = x[idx];
            BOOST_CHECK_CLOSE(v[0], sin(X), 1e-8);
            BOOST_CHECK_CLOSE(v[1], cos(X), 1e-8);
            });
}

#if (BOOST_VERSION >= 105200) && !defined(_MSC_VER)
BOOST_AUTO_TEST_CASE(multivector_temporary)
{
    typedef std::array<double, 2> elem_t;
    const size_t n = 1024;

    vex::multivector<double, 2> x(ctx, random_vector<double>(2 * n));
    vex::multivector<double, 2> y(ctx,n);

    auto tmp = vex::make_temp<1, double>( tan(x) );

    y = tmp * tmp;

    check_sample(y, [&](size_t idx, elem_t v){
            elem_t X = x[idx];

            BOOST_CHECK_CLOSE(v[0], pow(tan(X[0]), 2.0), 1e-8);
            BOOST_CHECK_CLOSE(v[1], pow(tan(X[1]), 2.0), 1e-8);
            });
}
#endif

BOOST_AUTO_TEST_SUITE_END()


#include <algorithm>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

#include <yaml-cpp/yaml.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>

/**
 * @brief Stereo camera parameters
 *
 */
struct StereoParam {
  // extrinsic matrix, transform right camera frame into left camera frame
  Eigen::Matrix4d T_r_l;
  // left camera intrinsic
  Eigen::Matrix3d cam0_intrinsic;
  // left camera distortion_coeffs
  double cam0_distortions[4];

  // right camera intrinsic
  Eigen::Matrix3d cam1_intrinsic;
  // right camera distortion_coeffs
  double cam1_distortions[4];

  int img_width;
  int img_height;
  // camera model
  std::string camera_type;
  // assume cam0 and cam1 use identical distortion type
  std::string distortion_type;

  StereoParam()
      : T_r_l(Eigen::Matrix4d::Identity()),
        cam0_intrinsic(Eigen::Matrix3d::Identity()),
        cam1_intrinsic(Eigen::Matrix3d::Identity()),
        img_width(2592),
        img_height(2048) {
    memset(cam0_distortions, 0, 4 * sizeof(double));
    memset(cam1_distortions, 0, 4 * sizeof(double));
    camera_type = "pinhole";
    distortion_type = "equi-distant";
  }

  void getExtrinsic(cv::Mat &R_rl, cv::Mat &t_rl)
  {
      Eigen::Matrix3d R = T_r_l.block<3, 3>(0, 0);
      Eigen::Vector3d t = T_r_l.block<3, 1>(0, 3);

      cv::eigen2cv(R, R_rl);
      cv::eigen2cv(t, t_rl);
  }

  void getCam0Param(cv::Mat &K, cv::Mat &D)
  {
      cv::eigen2cv(cam0_intrinsic, K);
      cv::Mat distortion = (cv::Mat_<double>(1, 4) << cam0_distortions[0], cam0_distortions[1],
              cam0_distortions[2], cam0_distortions[3]);
      D = distortion.clone();
  }

  void getCam1Param(cv::Mat &K, cv::Mat &D)
  {
      cv::eigen2cv(cam1_intrinsic, K);
      cv::Mat distortion = (cv::Mat_<double>(1, 4) << cam1_distortions[0], cam1_distortions[1],
              cam1_distortions[2], cam1_distortions[3]);
      D = distortion.clone();
  }
};

/**
 * @brief Monocular camera parameters
 *
 */
struct MonocularParam {
  // camera intrinsic
  Eigen::Matrix3d cam_intrinsic;
  // camera distortion_coeffs
  double cam_distortions[4];

  int img_width;
  int img_height;

  std::string distortion_type;
  // camera model
  std::string camera_type;

  MonocularParam()
      : cam_intrinsic(Eigen::Matrix3d::Identity()),
        img_width(2592),
        img_height(2048),
        distortion_type("equi-distant"),
        camera_type("pinhole") {
    memset(cam_distortions, 0, 4 * sizeof(double));
  }

  void getCamParam(cv::Mat& K, cv::Mat& D) {
    cv::eigen2cv(cam_intrinsic, K);
    cv::Mat distortion =
        (cv::Mat_<double>(1, 4) << cam_distortions[0], cam_distortions[1],
         cam_distortions[2], cam_distortions[3]);
    D = distortion.clone();
  }
};

/**
 * @brief Stereo-IMU parameters
 *
 */
struct StereoIMUParam {
  // cam0 2 imu
  Eigen::Matrix4d T_cam0_imu;
  // cam1 2 imu
  Eigen::Matrix4d T_cam1_imu;

  // acc bias(m/s^2)
  Eigen::Vector3d acc_bias;
  // gyro (rad/s)
  Eigen::Vector3d gyro_bias;

  // noise rms
  double avg_acc_noise_density;
  double avg_gyro_noise_density;
  // random walk noise rms
  double avg_acc_bias_noise_density;
  double avg_gyro_bias_noise_density;

  StereoIMUParam()
      : T_cam0_imu(Eigen::Matrix4d::Identity()),
        T_cam1_imu(Eigen::Matrix4d::Identity()),
        acc_bias(Eigen::Vector3d::Zero()),
        gyro_bias(Eigen::Vector3d::Zero()),
        avg_acc_noise_density(0.0),
        avg_gyro_noise_density(0.0),
        avg_acc_bias_noise_density(0.0),
        avg_gyro_bias_noise_density(0.0) {}
};

/**
 * @brief parse kalibr yaml file support: pinhole + equidistant
 *
 * @param kalib_intrin_file
 * @param camera Camera Model param
 */
bool readKalibrStereoOutputFile(const std::string& kalib_result_file,
                                StereoParam& stereo_param) {
  if (kalib_result_file.empty()) {
    std::cout << "[readKalibrOutputFile] Empty input file name!\n";
    return false;
  }

  std::ifstream fs(kalib_result_file);
  if (!fs.is_open()) {
    std::cout << "[readKalibrOutputFile] Error read " << kalib_result_file
              << "\n";
    return false;
  }

  YAML::Node root_node;
  YAML::Node cam0_node;
  YAML::Node cam1_node;

  try {
    root_node = YAML::LoadFile(kalib_result_file);
  } catch (YAML::BadFile& e) {
    std::cout << "[readKalibrOutputFile] Could not open file: "
              << kalib_result_file << "\n";
    return false;
  } catch (YAML::ParserException& e) {
    std::cout << "[readKalibrOutputFile] Invalid file format: "
              << kalib_result_file << "\n";
    return false;
  }
  if (root_node.IsNull()) {
    std::cout << "[loadKalibrResult] Could not open file: " << kalib_result_file
              << "\n";
    return false;
  }

  //    std::cout << "[readKalibrOutputFile] read "  << kalib_result_file <<
  //    ":\n";
  int img_width = 0, img_height = 0;

  cam0_node = root_node["cam0"];
  double cam0_distortion[4] = {0.0};
  cam0_distortion[0] = cam0_node["distortion_coeffs"][0].as<double>();
  cam0_distortion[1] = cam0_node["distortion_coeffs"][1].as<double>();
  cam0_distortion[2] = cam0_node["distortion_coeffs"][2].as<double>();
  cam0_distortion[3] = cam0_node["distortion_coeffs"][3].as<double>();

  Eigen::Matrix3d cam0_intrinsic = Eigen::Matrix3d::Identity();
  cam0_intrinsic(0, 0) = cam0_node["intrinsics"][0].as<double>();
  cam0_intrinsic(1, 1) = cam0_node["intrinsics"][1].as<double>();
  cam0_intrinsic(0, 2) = cam0_node["intrinsics"][2].as<double>();
  cam0_intrinsic(1, 2) = cam0_node["intrinsics"][3].as<double>();

  cam1_node = root_node["cam1"];
  double cam1_distortion[4] = {0.0};
  cam1_distortion[0] = cam1_node["distortion_coeffs"][0].as<double>();
  cam1_distortion[1] = cam1_node["distortion_coeffs"][1].as<double>();
  cam1_distortion[2] = cam1_node["distortion_coeffs"][2].as<double>();
  cam1_distortion[3] = cam1_node["distortion_coeffs"][3].as<double>();

  Eigen::Matrix3d cam1_intrinsic = Eigen::Matrix3d::Identity();
  cam1_intrinsic(0, 0) = cam1_node["intrinsics"][0].as<double>();
  cam1_intrinsic(1, 1) = cam1_node["intrinsics"][1].as<double>();
  cam1_intrinsic(0, 2) = cam1_node["intrinsics"][2].as<double>();
  cam1_intrinsic(1, 2) = cam1_node["intrinsics"][3].as<double>();

  img_width = cam1_node["resolution"][0].as<int>();
  img_height = cam1_node["resolution"][1].as<int>();

  std::string distortion_type = cam0_node["distortion_model"].as<std::string>();
  std::string camera_type = cam0_node["camera_model"].as<std::string>();

  Eigen::Matrix4d extrinsic = Eigen::Matrix4d::Identity();
  for (size_t i = 0; i < cam1_node["T_cn_cnm1"].size(); ++i) {
    extrinsic(i, 0) = cam1_node["T_cn_cnm1"][i][0].as<double>();
    extrinsic(i, 1) = cam1_node["T_cn_cnm1"][i][1].as<double>();
    extrinsic(i, 2) = cam1_node["T_cn_cnm1"][i][2].as<double>();
    extrinsic(i, 3) = cam1_node["T_cn_cnm1"][i][3].as<double>();
  }

  stereo_param.T_r_l = extrinsic;
  //    std::cout << "[readKalibrOutputFile] T_r_l : \n" << stereo_param.T_r_l
  //    << "\n";
  Eigen::Vector3d baseline = stereo_param.T_r_l.block<3, 1>(0, 3);
  //    std::cout << "[readKalibrOutputFile] baseline norm : " <<
  //    baseline.norm() << "\n";

  //    std::cout << "[loadKalibrResult] baseline * fx = " << (baseline.norm() *
  //    cam0_intrinsic(0, 0)) << "\n";

  stereo_param.cam0_intrinsic = cam0_intrinsic;
  //    std::cout << "[loadKalibrResult] cam0 intrinsic : \n" <<
  //    stereo_param.cam0_intrinsic << "\n";
  memcpy(stereo_param.cam0_distortions, cam0_distortion, 4 * sizeof(double));
  //    std::cout << "[loadKalibrResult] cam0 distrotion : " <<
  //    stereo_param.cam0_distortions[0] << ", " <<
  //    stereo_param.cam0_distortions[3] << "\n";
  stereo_param.cam1_intrinsic = cam1_intrinsic;
  //    std::cout << "[loadKalibrResult] cam1 intrinsic : \n" <<
  //    stereo_param.cam1_intrinsic << "\n";
  memcpy(stereo_param.cam1_distortions, cam1_distortion, 4 * sizeof(double));
  //    std::cout << "[loadKalibrResult] cam1 distrotion : " <<
  //    stereo_param.cam1_distortions[0] << ", " <<
  //    stereo_param.cam1_distortions[3] << "\n";
  stereo_param.img_width = img_width;
  stereo_param.img_height = img_height;
  stereo_param.distortion_type = distortion_type;
  stereo_param.camera_type = camera_type;

  return true;
}

bool readKalibrMonoOutputFile(const std::string& kalib_result_file,
                              MonocularParam& mono_param) {
  if (kalib_result_file.empty()) {
    std::cout << "[readKalibrOutputFile] Empty input file name!\n";
    return false;
  }

  std::ifstream fs(kalib_result_file);
  if (!fs.is_open()) {
    std::cout << "[readKalibrOutputFile] Error read " << kalib_result_file
              << "\n";
    return false;
  }

  YAML::Node root_node;
  YAML::Node cam0_node;

  try {
    root_node = YAML::LoadFile(kalib_result_file);
  } catch (YAML::BadFile& e) {
    std::cout << "[readKalibrOutputFile] Could not open file: "
              << kalib_result_file << "\n";
    return false;
  } catch (YAML::ParserException& e) {
    std::cout << "[readKalibrOutputFile] Invalid file format: "
              << kalib_result_file << "\n";
    return false;
  }
  if (root_node.IsNull()) {
    std::cout << "[loadKalibrResult] Could not open file: " << kalib_result_file
              << "\n";
    return false;
  }

  //    std::cout << "[readKalibrOutputFile] read "  << kalib_result_file <<
  //    ":\n";
  int img_width = 0, img_height = 0;

  cam0_node = root_node["cam0"];
  double cam0_distortion[4] = {0.0};
  cam0_distortion[0] = cam0_node["distortion_coeffs"][0].as<double>();
  cam0_distortion[1] = cam0_node["distortion_coeffs"][1].as<double>();
  cam0_distortion[2] = cam0_node["distortion_coeffs"][2].as<double>();
  cam0_distortion[3] = cam0_node["distortion_coeffs"][3].as<double>();

  Eigen::Matrix3d cam0_intrinsic = Eigen::Matrix3d::Identity();
  cam0_intrinsic(0, 0) = cam0_node["intrinsics"][0].as<double>();
  cam0_intrinsic(1, 1) = cam0_node["intrinsics"][1].as<double>();
  cam0_intrinsic(0, 2) = cam0_node["intrinsics"][2].as<double>();
  cam0_intrinsic(1, 2) = cam0_node["intrinsics"][3].as<double>();

  img_width = cam0_node["resolution"][0].as<int>();
  img_height = cam0_node["resolution"][1].as<int>();

  std::string distortion_type = cam0_node["distortion_model"].as<std::string>();
  std::string camera_type = cam0_node["camera_model"].as<std::string>();

  mono_param.cam_intrinsic = cam0_intrinsic;
  //    std::cout << "[loadKalibrResult] cam0 intrinsic : \n" <<
  //    mono_param.cam0_intrinsic << "\n";
  memcpy(mono_param.cam_distortions, cam0_distortion, 4 * sizeof(double));
  //    std::cout << "[loadKalibrResult] cam0 distrotion : " <<
  //    mono_param.cam0_distortions[0] << ", " << mono_param.cam0_distortions[3]
  //    << "\n";
  mono_param.img_width = img_width;
  mono_param.img_height = img_height;
  mono_param.distortion_type = distortion_type;
  mono_param.camera_type = camera_type;

  return true;
}

bool readKalibrStereoIMUOutputFile(const std::string& kalib_result_file,
                                   StereoIMUParam& stereo_imu_param) {
  if (kalib_result_file.empty()) {
    std::cout << "[readKalibrStereoIMUOutputFile] Empty input file name!\n";
    return false;
  }

  std::ifstream fs(kalib_result_file);
  if (!fs.is_open()) {
    std::cout << "[readKalibrStereoIMUOutputFile] Error read "
              << kalib_result_file << "\n";
    return false;
  }

  YAML::Node root_node;

  try {
    root_node = YAML::LoadFile(kalib_result_file);
  } catch (YAML::BadFile& e) {
    std::cout << "[readKalibrStereoIMUOutputFile] Could not open file: "
              << kalib_result_file << "\n";
    return false;
  } catch (YAML::ParserException& e) {
    std::cout << "[readKalibrStereoIMUOutputFile] Invalid file format: "
              << kalib_result_file << "\n";
    return false;
  }
  if (root_node.IsNull()) {
    std::cout << "[readKalibrStereoIMUOutputFile] Could not open file: "
              << kalib_result_file << "\n";
    return false;
  }

  std::cout << "[readKalibrStereoIMUOutputFile] read " << kalib_result_file
            << "\n";

  YAML::Node cam0_node = root_node["cam0"];
  Eigen::Matrix4d extrinsic = Eigen::Matrix4d::Identity();
  for (size_t i = 0; i < cam0_node["T_cam_imu"].size(); ++i) {
    extrinsic(i, 0) = cam0_node["T_cam_imu"][i][0].as<double>();
    extrinsic(i, 1) = cam0_node["T_cam_imu"][i][1].as<double>();
    extrinsic(i, 2) = cam0_node["T_cam_imu"][i][2].as<double>();
    extrinsic(i, 3) = cam0_node["T_cam_imu"][i][3].as<double>();
  }

  stereo_imu_param.T_cam0_imu = extrinsic;

  YAML::Node cam1_node = root_node["cam1"];
  for (size_t i = 0; i < cam1_node["T_cam_imu"].size(); ++i) {
    extrinsic(i, 0) = cam1_node["T_cam_imu"][i][0].as<double>();
    extrinsic(i, 1) = cam1_node["T_cam_imu"][i][1].as<double>();
    extrinsic(i, 2) = cam1_node["T_cam_imu"][i][2].as<double>();
    extrinsic(i, 3) = cam1_node["T_cam_imu"][i][3].as<double>();
  }

  stereo_imu_param.T_cam1_imu = extrinsic;

  std::cout << "[readKalibrStereoIMUOutputFile] camera0-IMU : \n"
            << stereo_imu_param.T_cam0_imu << "\n";
  std::cout << "[readKalibrStereoIMUOutputFile] camera1-IMU : \n"
            << stereo_imu_param.T_cam1_imu << "\n";
  return true;
}

/**
 * @brief Write mono-camera parameters
 *
 * @param file_name is the output yaml file name.
 * @param mono_param is the monocular camera parameters.
 */
bool writeCamParam(const std::string& file_name, MonocularParam& mono_param) {
  if (file_name.empty()) {
    std::cout << "[writeCamParam] Empty file name!\n";
    return false;
  }

  cv::FileStorage fs(file_name, cv::FileStorage::WRITE);
  if (!fs.isOpened()) {
    std::cout << "[writeCamParam] Fail to open " << file_name << "\n";
    return false;
  }

  fs << "camera_model" << mono_param.distortion_type;

  cv::Mat K, D;
  //
  {
    mono_param.getCamParam(K, D);
    fs << "camera_matrix" << K;
    fs << "distortion_coefficients" << D;
  }

  fs << "avg_reprojection_error" << 1.2799437834241592e-01;
  fs << "image_width" << mono_param.img_width;
  fs << "image_height" << mono_param.img_height;
  fs << "serial_num" << 1;
  fs.release();
  return true;
}

/**
 * @brief Write stereo-camera parameters
 *
 * @param file_name is the output yaml file name.
 * @param cam_index is .
 * @param mono_param is the monocular camera parameters.
 */
bool writeCamParam(const std::string &file_name, const int cam_index, StereoParam &stereo_param)
{
    if (file_name.empty())
    {
        std::cout << "[writeCamParam] Empty file name!\n";
        return false;
    }

    cv::FileStorage fs(file_name, cv::FileStorage::WRITE);
    if (!fs.isOpened())
    {
        std::cout << "[writeCamParam] Fail to open " << file_name << "\n";
        return false;
    }

    fs << "image_width" << stereo_param.img_width;
    fs << "image_height" << stereo_param.img_height;
    fs << "distortion_type" << stereo_param.distortion_type;

    cv::Mat K, D;
    //
    if (0 == cam_index)
    {
        stereo_param.getCam0Param(K, D);
        fs << "camera_matrix" << K;
        fs << "distortion_coefficients" << D;
    }
    if (1 == cam_index)
    {
        stereo_param.getCam1Param(K, D);
        fs << "camera_matrix" << K;
        fs << "distortion_coefficients" << D;
    }

    fs.release();
    return true;
}

bool analyzeKalibrOutput(const std::vector<StereoParam>& v_stereo_params,
                         const std::string& output_fn,
                         const std::string& result_fn) {
  if (v_stereo_params.empty()) {
    std::cerr << "[analyKalibrOutput] Input is empty!\n";
    return false;
  }
  if (output_fn.empty()) {
    std::cerr << "[analyzeKalibrOutput] output file name is empty!\n";
    return false;
  }

  std::ofstream ofs(output_fn);
  if (!ofs.is_open()) {
    std::cerr << "[analyzeKalibrOutput] Fail to open " << output_fn << "\n";
    return false;
  }

  // fx,fy,cx,cy
  double avg_cam0_intrinsic_params[4] = {0.0};
  double avg_cam0_distortion_params[4] = {0.0};
  // k1,k2,k3,k4
  double avg_cam1_intrinsic_params[4] = {0.0};
  double avg_cam1_distortion_params[4] = {0.0};

  // calculate mean
  for (size_t i = 0; i < v_stereo_params.size(); ++i) {
    // cam0
    avg_cam0_intrinsic_params[0] += v_stereo_params[i].cam0_intrinsic(0, 0);
    avg_cam0_intrinsic_params[1] += v_stereo_params[i].cam0_intrinsic(1, 1);
    avg_cam0_intrinsic_params[2] += v_stereo_params[i].cam0_intrinsic(0, 2);
    avg_cam0_intrinsic_params[3] += v_stereo_params[i].cam0_intrinsic(1, 2);
    avg_cam0_distortion_params[0] += v_stereo_params[i].cam0_distortions[0];
    avg_cam0_distortion_params[1] += v_stereo_params[i].cam0_distortions[1];
    avg_cam0_distortion_params[2] += v_stereo_params[i].cam0_distortions[2];
    avg_cam0_distortion_params[3] += v_stereo_params[i].cam0_distortions[3];

    // cam1
    avg_cam1_intrinsic_params[0] += v_stereo_params[i].cam1_intrinsic(0, 0);
    avg_cam1_intrinsic_params[1] += v_stereo_params[i].cam1_intrinsic(1, 1);
    avg_cam1_intrinsic_params[2] += v_stereo_params[i].cam1_intrinsic(0, 2);
    avg_cam1_intrinsic_params[3] += v_stereo_params[i].cam1_intrinsic(1, 2);
    avg_cam1_distortion_params[0] += v_stereo_params[i].cam1_distortions[0];
    avg_cam1_distortion_params[1] += v_stereo_params[i].cam1_distortions[1];
    avg_cam1_distortion_params[2] += v_stereo_params[i].cam1_distortions[2];
    avg_cam1_distortion_params[3] += v_stereo_params[i].cam1_distortions[3];
  }

  int num = v_stereo_params.size();

  for (int i = 0; i < 4; ++i) {
    avg_cam0_intrinsic_params[i] /= num;
    avg_cam0_distortion_params[i] /= num;
    avg_cam1_intrinsic_params[i] /= num;
    avg_cam1_distortion_params[i] /= num;
  }

  std::cout << "[analyzeKalibrOutput] cam0 Average fx : "
            << avg_cam0_intrinsic_params[0] << "\n";
  ofs << "cam0 Average fx : " << avg_cam0_intrinsic_params[0] << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 Average fy : "
            << avg_cam0_intrinsic_params[1] << "\n";
  ofs << "cam0 Average fy : " << avg_cam0_intrinsic_params[1] << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 Average cx : "
            << avg_cam0_intrinsic_params[2] << "\n";
  ofs << "cam0 Average cx : " << avg_cam0_intrinsic_params[2] << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 Average cy : "
            << avg_cam0_intrinsic_params[3] << "\n";
  ofs << "cam0 Average cy : " << avg_cam0_intrinsic_params[3] << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 Average k1 : "
            << avg_cam0_distortion_params[0] << "\n";
  ofs << "cam0 Average k1 : " << avg_cam0_distortion_params[0] << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 Average k2 : "
            << avg_cam0_distortion_params[1] << "\n";
  ofs << "cam0 Average k2 : " << avg_cam0_distortion_params[1] << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 Average k3 : "
            << avg_cam0_distortion_params[2] << "\n";
  ofs << "cam0 Average k3 : " << avg_cam0_distortion_params[2] << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 Average k4 : "
            << avg_cam0_distortion_params[3] << "\n";
  ofs << "cam0 Average k4 : " << avg_cam0_distortion_params[3] << "\n";

  std::cout << "[analyzeKalibrOutput] cam1 Average fx : "
            << avg_cam1_intrinsic_params[0] << "\n";
  ofs << "cam1 Average fx : " << avg_cam1_intrinsic_params[0] << "\n";
  std::cout << "[analyzeKalibrOutput] cam1 Average fy : "
            << avg_cam1_intrinsic_params[1] << "\n";
  ofs << "cam1 Average fy : " << avg_cam1_intrinsic_params[1] << "\n";
  std::cout << "[analyzeKalibrOutput] cam1 Average cx : "
            << avg_cam1_intrinsic_params[2] << "\n";
  ofs << "cam1 Average cx : " << avg_cam1_intrinsic_params[2] << "\n";
  std::cout << "[analyzeKalibrOutput] cam1 Average cy : "
            << avg_cam1_intrinsic_params[3] << "\n";
  ofs << "cam1 Average cy : " << avg_cam1_intrinsic_params[3] << "\n";
  std::cout << "[analyzeKalibrOutput] cam1 Average k1 : "
            << avg_cam1_distortion_params[0] << "\n";
  ofs << "cam1 Average k1 : " << avg_cam1_distortion_params[0] << "\n";
  std::cout << "[analyzeKalibrOutput] cam1 Average k2 : "
            << avg_cam1_distortion_params[1] << "\n";
  ofs << "cam1 Average k2 : " << avg_cam1_distortion_params[1] << "\n";
  std::cout << "[analyzeKalibrOutput] cam1 Average k3 : "
            << avg_cam1_distortion_params[2] << "\n";
  ofs << "cam1 Average k3 : " << avg_cam1_distortion_params[2] << "\n";
  std::cout << "[analyzeKalibrOutput] cam1 Average k4 : "
            << avg_cam1_distortion_params[3] << "\n";
  ofs << "cam1 Average k4 : " << avg_cam1_distortion_params[3] << "\n";

  // calculate RMS
  double fx_rms[2] = {0}, fy_rms[2] = {0}, cx_rms[2] = {0}, cy_rms[2] = {0};
  double k1_rms[2] = {0}, k2_rms[2] = {0}, k3_rms[2] = {0}, k4_rms[2] = {0};
  for (size_t i = 0; i < v_stereo_params.size(); ++i) {
    // cam0
    double err_fx =
        v_stereo_params[i].cam0_intrinsic(0, 0) - avg_cam0_intrinsic_params[0];
    double err_fy =
        v_stereo_params[i].cam0_intrinsic(1, 1) - avg_cam0_intrinsic_params[1];
    double err_cx =
        v_stereo_params[i].cam0_intrinsic(0, 2) - avg_cam0_intrinsic_params[2];
    double err_cy =
        v_stereo_params[i].cam0_intrinsic(1, 2) - avg_cam0_intrinsic_params[3];

    fx_rms[0] += err_fx * err_fx;
    fy_rms[0] += err_fy * err_fy;
    cx_rms[0] += err_cx * err_cx;
    cy_rms[0] += err_cy * err_cy;

    double err_k1 =
        v_stereo_params[i].cam0_distortions[0] - avg_cam0_distortion_params[0];
    double err_k2 =
        v_stereo_params[i].cam0_distortions[1] - avg_cam0_distortion_params[1];
    double err_k3 =
        v_stereo_params[i].cam0_distortions[2] - avg_cam0_distortion_params[2];
    double err_k4 =
        v_stereo_params[i].cam0_distortions[3] - avg_cam0_distortion_params[3];

    k1_rms[0] += err_k1 * err_k1;
    k2_rms[0] += err_k2 * err_k2;
    k3_rms[0] += err_k3 * err_k3;
    k4_rms[0] += err_k4 * err_k4;

    // cam1
    err_fx =
        v_stereo_params[i].cam1_intrinsic(0, 0) - avg_cam1_intrinsic_params[0];
    err_fy =
        v_stereo_params[i].cam1_intrinsic(1, 1) - avg_cam1_intrinsic_params[1];
    err_cx =
        v_stereo_params[i].cam1_intrinsic(0, 2) - avg_cam1_intrinsic_params[2];
    err_cy =
        v_stereo_params[i].cam1_intrinsic(1, 2) - avg_cam1_intrinsic_params[3];

    fx_rms[1] += err_fx * err_fx;
    fy_rms[1] += err_fy * err_fy;
    cx_rms[1] += err_cx * err_cx;
    cy_rms[1] += err_cy * err_cy;

    err_k1 =
        v_stereo_params[i].cam1_distortions[0] - avg_cam1_distortion_params[0];
    err_k2 =
        v_stereo_params[i].cam1_distortions[1] - avg_cam1_distortion_params[1];
    err_k3 =
        v_stereo_params[i].cam1_distortions[2] - avg_cam1_distortion_params[2];
    err_k4 =
        v_stereo_params[i].cam1_distortions[3] - avg_cam1_distortion_params[3];

    k1_rms[1] += err_k1 * err_k1;
    k2_rms[1] += err_k2 * err_k2;
    k3_rms[1] += err_k3 * err_k3;
    k4_rms[1] += err_k4 * err_k4;
  }

  fx_rms[0] /= num;
  fx_rms[1] /= num;
  fy_rms[0] /= num;
  fy_rms[1] /= num;
  cx_rms[0] /= num;
  cx_rms[1] /= num;
  cy_rms[0] /= num;
  cy_rms[1] /= num;
  k1_rms[0] /= num;
  k1_rms[1] /= num;
  k2_rms[0] /= num;
  k2_rms[1] /= num;
  k3_rms[0] /= num;
  k3_rms[1] /= num;
  k4_rms[0] /= num;
  k4_rms[1] /= num;

  std::cout << "[analyzeKalibrOutput] cam0 fx rms: " << std::sqrt(fx_rms[0])
            << "\n";
  ofs << "cam0 fx rms: " << std::sqrt(fx_rms[0]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 fy rms: " << std::sqrt(fy_rms[0])
            << "\n";
  ofs << "cam0 fy rms: " << std::sqrt(fy_rms[0]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 cx rms: " << std::sqrt(cx_rms[0])
            << "\n";
  ofs << "cam0 cx rms: " << std::sqrt(cx_rms[0]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 cy rms: " << std::sqrt(cy_rms[0])
            << "\n";
  ofs << "cam0 cy rms: " << std::sqrt(cy_rms[0]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 k1 rms: " << std::sqrt(k1_rms[0])
            << "\n";
  ofs << "cam0 k1 rms: " << std::sqrt(k1_rms[0]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 k2 rms: " << std::sqrt(k2_rms[0])
            << "\n";
  ofs << "cam0 k2 rms: " << std::sqrt(k2_rms[0]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 k3 rms: " << std::sqrt(k3_rms[0])
            << "\n";
  ofs << "cam0 k3 rms: " << std::sqrt(k3_rms[0]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 k4 rms: " << std::sqrt(k4_rms[0])
            << "\n";
  ofs << "cam0 k4 rms: " << std::sqrt(k4_rms[0]) << "\n";

  std::cout << "[analyzeKalibrOutput] cam1 fx rms: " << std::sqrt(fx_rms[1])
            << "\n";
  ofs << "cam1 fx rms: " << std::sqrt(fx_rms[1]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam1 fy rms: " << std::sqrt(fy_rms[1])
            << "\n";
  ofs << "cam1 fy rms: " << std::sqrt(fy_rms[1]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam1 cx rms: " << std::sqrt(cx_rms[1])
            << "\n";
  ofs << "cam1 cx rms: " << std::sqrt(cx_rms[1]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam1 cy rms: " << std::sqrt(cy_rms[1])
            << "\n";
  ofs << "cam1 cy rms: " << std::sqrt(cy_rms[1]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam1 k1 rms: " << std::sqrt(k1_rms[1])
            << "\n";
  ofs << "cam1 k1 rms: " << std::sqrt(k1_rms[1]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam1 k2 rms: " << std::sqrt(k2_rms[1])
            << "\n";
  ofs << "cam1 k2 rms: " << std::sqrt(k2_rms[1]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam1 k3 rms: " << std::sqrt(k3_rms[1])
            << "\n";
  ofs << "cam1 k3 rms: " << std::sqrt(k3_rms[1]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam1 k4 rms: " << std::sqrt(k4_rms[1])
            << "\n";
  ofs << "cam1 k4 rms: " << std::sqrt(k4_rms[1]) << "\n";

  assert(v_stereo_params.size() > 1);
  // avg delta R
  double avg_extrinsic_delta_rotation = 0.0;
  // avh delta t
  double avg_extrinsic_delta_t = 0.0;

  Eigen::Matrix4d reference_T_r_l_inv = v_stereo_params[0].T_r_l.inverse();
  for (int i = 0; i < num; ++i) {
    Eigen::Matrix4d T_r_l = v_stereo_params[i].T_r_l;
    Eigen::Matrix4d delta_T = T_r_l * reference_T_r_l_inv;
    Eigen::Matrix3d delta_R = delta_T.block<3, 3>(0, 0);
    Eigen::Vector3d delta_t = delta_T.block<3, 1>(0, 3);

    Eigen::AngleAxisd R_vec(delta_R);
    avg_extrinsic_delta_rotation += R_vec.angle() * 180.0 / M_PI;
    avg_extrinsic_delta_t += delta_t.norm();
  }

  std::cout << "[annalyzeKalibrOutput] Average T_r_l rotation error : "
            << avg_extrinsic_delta_rotation / num << "\n";
  ofs << "Average T_r_l rotation error : " << avg_extrinsic_delta_rotation / num
      << "\n";
  std::cout << "[annalyzeKalibrOutput] Average T_r_l translation error : "
            << avg_extrinsic_delta_t / num << "\n";
  ofs << "Average T_r_l translation error : " << avg_extrinsic_delta_t / num
      << "\n";

  ofs.close();

//  StereoParam avg_stereo_param;
//  avg_stereo_param.T_r_l = v_stereo_params[0].T_r_l;
//  avg_stereo_param.img_width = v_stereo_params[0].img_width;
//  avg_stereo_param.img_height = v_stereo_params[0].img_height;
//  avg_stereo_param.camera_type = v_stereo_params[0].camera_type;
//  avg_stereo_param.distortion_type = v_stereo_params[0].distortion_type;
//  avg_stereo_param.cam0_intrinsic(0, 0) = avg_cam0_intrinsic_params[0];
//  avg_stereo_param.cam0_intrinsic(1, 1) = avg_cam0_intrinsic_params[1];
//  avg_stereo_param.cam0_intrinsic(0, 2) = avg_cam0_intrinsic_params[2];
//  avg_stereo_param.cam0_intrinsic(1, 2) = avg_cam0_intrinsic_params[3];
//  avg_stereo_param.cam0_distortions[0] = avg_cam0_distortion_params[0];
//  avg_stereo_param.cam0_distortions[1] = avg_cam0_distortion_params[1];
//  avg_stereo_param.cam0_distortions[2] = avg_cam0_distortion_params[2];
//  avg_stereo_param.cam0_distortions[3] = avg_cam0_distortion_params[3];
//  avg_stereo_param.cam1_intrinsic(0, 0) = avg_cam1_intrinsic_params[0];
//  avg_stereo_param.cam1_intrinsic(1, 1) = avg_cam1_intrinsic_params[1];
//  avg_stereo_param.cam1_intrinsic(0, 2) = avg_cam1_intrinsic_params[2];
//  avg_stereo_param.cam1_intrinsic(1, 2) = avg_cam1_intrinsic_params[3];
//  avg_stereo_param.cam1_distortions[0] = avg_cam1_distortion_params[0];
//  avg_stereo_param.cam1_distortions[1] = avg_cam1_distortion_params[1];
//  avg_stereo_param.cam1_distortions[2] = avg_cam1_distortion_params[2];
//  avg_stereo_param.cam1_distortions[3] = avg_cam1_distortion_params[3];
//  writeCamParam (result_fn, avg_stereo_param);
  return true;
}

bool analyzeKalibrOutput(const std::vector<MonocularParam>& v_mono_params,
                         const std::string& output_fn,
                         const std::string& result_fn) {
  if (v_mono_params.empty()) {
    std::cerr << "[analyKalibrOutput] Input is empty!\n";
    return false;
  }
  if (output_fn.empty()) {
    std::cerr << "[analyzeKalibrOutput] output file name is empty!\n";
    return false;
  }

  std::ofstream ofs(output_fn);
  if (!ofs.is_open()) {
    std::cerr << "[analyzeKalibrOutput] Fail to open " << output_fn << "\n";
    return false;
  }

  // fx,fy,cx,cy
  double avg_cam0_intrinsic_params[4] = {0.0};
  // k1,k2,k3,k4
  double avg_cam0_distortion_params[4] = {0.0};

  // calculate mean
  for (size_t i = 0; i < v_mono_params.size(); ++i) {
    // cam0
    avg_cam0_intrinsic_params[0] += v_mono_params[i].cam_intrinsic(0, 0);
    avg_cam0_intrinsic_params[1] += v_mono_params[i].cam_intrinsic(1, 1);
    avg_cam0_intrinsic_params[2] += v_mono_params[i].cam_intrinsic(0, 2);
    avg_cam0_intrinsic_params[3] += v_mono_params[i].cam_intrinsic(1, 2);
    avg_cam0_distortion_params[0] += v_mono_params[i].cam_distortions[0];
    avg_cam0_distortion_params[1] += v_mono_params[i].cam_distortions[1];
    avg_cam0_distortion_params[2] += v_mono_params[i].cam_distortions[2];
    avg_cam0_distortion_params[3] += v_mono_params[i].cam_distortions[3];
  }

  int num = v_mono_params.size();

  for (int i = 0; i < 4; ++i) {
    avg_cam0_intrinsic_params[i] /= num;
    avg_cam0_distortion_params[i] /= num;
  }

  MonocularParam avg_cam_param;
  avg_cam_param.img_height = v_mono_params[0].img_height;
  avg_cam_param.img_width = v_mono_params[0].img_width;
  avg_cam_param.cam_intrinsic(0, 0) = avg_cam0_intrinsic_params[0];
  avg_cam_param.cam_intrinsic(1, 1) = avg_cam0_intrinsic_params[1];
  avg_cam_param.cam_intrinsic(0, 2) = avg_cam0_intrinsic_params[2];
  avg_cam_param.cam_intrinsic(1, 2) = avg_cam0_intrinsic_params[3];
  avg_cam_param.camera_type = v_mono_params[0].camera_type;
  avg_cam_param.distortion_type = v_mono_params[0].distortion_type;
  avg_cam_param.cam_distortions[0] = avg_cam0_distortion_params[0];
  avg_cam_param.cam_distortions[1] = avg_cam0_distortion_params[1];
  avg_cam_param.cam_distortions[2] = avg_cam0_distortion_params[2];
  avg_cam_param.cam_distortions[3] = avg_cam0_distortion_params[3];
  writeCamParam(result_fn, avg_cam_param);

  std::cout << "[analyzeKalibrOutput] cam0 Average fx : "
            << avg_cam0_intrinsic_params[0] << "\n";
  ofs << "cam0 Average fx : " << avg_cam0_intrinsic_params[0] << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 Average fy : "
            << avg_cam0_intrinsic_params[1] << "\n";
  ofs << "cam0 Average fy : " << avg_cam0_intrinsic_params[1] << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 Average cx : "
            << avg_cam0_intrinsic_params[2] << "\n";
  ofs << "cam0 Average cx : " << avg_cam0_intrinsic_params[2] << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 Average cy : "
            << avg_cam0_intrinsic_params[3] << "\n";
  ofs << "cam0 Average cy : " << avg_cam0_intrinsic_params[3] << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 Average k1 : "
            << avg_cam0_distortion_params[0] << "\n";
  ofs << "cam0 Average k1 : " << avg_cam0_distortion_params[0] << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 Average k2 : "
            << avg_cam0_distortion_params[1] << "\n";
  ofs << "cam0 Average k2 : " << avg_cam0_distortion_params[1] << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 Average k3 : "
            << avg_cam0_distortion_params[2] << "\n";
  ofs << "cam0 Average k3 : " << avg_cam0_distortion_params[2] << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 Average k4 : "
            << avg_cam0_distortion_params[3] << "\n";
  ofs << "cam0 Average k4 : " << avg_cam0_distortion_params[3] << "\n";

  // calculate RMS
  double fx_rms[2] = {0}, fy_rms[2] = {0}, cx_rms[2] = {0}, cy_rms[2] = {0};
  double k1_rms[2] = {0}, k2_rms[2] = {0}, k3_rms[2] = {0}, k4_rms[2] = {0};
  for (size_t i = 0; i < v_mono_params.size(); ++i) {
    // cam0
    double err_fx =
        v_mono_params[i].cam_intrinsic(0, 0) - avg_cam0_intrinsic_params[0];
    double err_fy =
        v_mono_params[i].cam_intrinsic(1, 1) - avg_cam0_intrinsic_params[1];
    double err_cx =
        v_mono_params[i].cam_intrinsic(0, 2) - avg_cam0_intrinsic_params[2];
    double err_cy =
        v_mono_params[i].cam_intrinsic(1, 2) - avg_cam0_intrinsic_params[3];

    fx_rms[0] += err_fx * err_fx;
    fy_rms[0] += err_fy * err_fy;
    cx_rms[0] += err_cx * err_cx;
    cy_rms[0] += err_cy * err_cy;

    double err_k1 =
        v_mono_params[i].cam_distortions[0] - avg_cam0_distortion_params[0];
    double err_k2 =
        v_mono_params[i].cam_distortions[1] - avg_cam0_distortion_params[1];
    double err_k3 =
        v_mono_params[i].cam_distortions[2] - avg_cam0_distortion_params[2];
    double err_k4 =
        v_mono_params[i].cam_distortions[3] - avg_cam0_distortion_params[3];

    k1_rms[0] += err_k1 * err_k1;
    k2_rms[0] += err_k2 * err_k2;
    k3_rms[0] += err_k3 * err_k3;
    k4_rms[0] += err_k4 * err_k4;
  }

  fx_rms[0] /= num;
  fy_rms[0] /= num;
  cx_rms[0] /= num;
  cy_rms[0] /= num;
  k1_rms[0] /= num;
  k2_rms[0] /= num;
  k3_rms[0] /= num;
  k4_rms[0] /= num;

  std::cout << "[analyzeKalibrOutput] cam0 fx rms: " << std::sqrt(fx_rms[0])
            << "\n";
  ofs << "cam0 fx rms: " << std::sqrt(fx_rms[0]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 fy rms: " << std::sqrt(fy_rms[0])
            << "\n";
  ofs << "cam0 fy rms: " << std::sqrt(fy_rms[0]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 cx rms: " << std::sqrt(cx_rms[0])
            << "\n";
  ofs << "cam0 cx rms: " << std::sqrt(cx_rms[0]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 cy rms: " << std::sqrt(cy_rms[0])
            << "\n";
  ofs << "cam0 cy rms: " << std::sqrt(cy_rms[0]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 k1 rms: " << std::sqrt(k1_rms[0])
            << "\n";
  ofs << "cam0 k1 rms: " << std::sqrt(k1_rms[0]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 k2 rms: " << std::sqrt(k2_rms[0])
            << "\n";
  ofs << "cam0 k2 rms: " << std::sqrt(k2_rms[0]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 k3 rms: " << std::sqrt(k3_rms[0])
            << "\n";
  ofs << "cam0 k3 rms: " << std::sqrt(k3_rms[0]) << "\n";
  std::cout << "[analyzeKalibrOutput] cam0 k4 rms: " << std::sqrt(k4_rms[0])
            << "\n";
  ofs << "cam0 k4 rms: " << std::sqrt(k4_rms[0]) << "\n";

  ofs.close();
  return true;
}

int main(int argc, char* argv[]) {
  if (argc < 3) {
    std::cerr << "Usage: test_eval_kalibr camera_model file_num "
                 "kalibr_file1 kalibr_file2 kalibr_file3 "
                 "analysis_file_name avg_result_file_name...\n";
    std::cerr << "\t camera_model : mono, stereo\n";
    return -1;
  }

  std::string camera_model(argv[1]);
  int file_num = std::stoi(argv[2]);
  std::vector<std::string> v_file_names(file_num);
  for (int i = 0; i < file_num; ++i) {
    v_file_names[i] = argv[i + 3];
  }
  std::string analysis_file_name(argv[file_num + 3]);
  std::string avg_result_file_name(argv[file_num + 4]);

  if (camera_model == "stereo") {
    std::vector<StereoParam> v_stereo_params;
    for (int i = 0; i < file_num; ++i) {
      StereoParam stereo_param;
      bool sts = readKalibrStereoOutputFile(v_file_names[i], stereo_param);
      if (!sts) {
        std::cerr << "[AnalyzeKalibrOutput] Fail to read " << v_file_names[i]
                  << "\n";
        return -1;
      }
      v_stereo_params.emplace_back(stereo_param);
    }

    bool sts = analyzeKalibrOutput(v_stereo_params, analysis_file_name,
                                   avg_result_file_name);
    if (!sts) {
      std::cerr << "[AnalyzeKalibrOutput] Fail to analyze statistic data from "
                   "kalibr stereo output files.\n";
      return -1;
    }
  } else if (camera_model == "mono") {
    std::vector<MonocularParam> v_mono_params;
    for (int i = 0; i < file_num; ++i) {
      MonocularParam mono_param;
      bool sts = readKalibrMonoOutputFile(v_file_names[i], mono_param);
      if (!sts) {
        std::cerr << "[AnalyzeKalibrOutput] Fail to read " << v_file_names[i]
                  << "\n";
        return -1;
      }
      v_mono_params.emplace_back(mono_param);
    }

    bool sts = analyzeKalibrOutput(v_mono_params, analysis_file_name,
                                   avg_result_file_name);
    if (!sts) {
      std::cerr << "[AnalyzeKalibrOutput] Fail to analyze statistic data from "
                   "kalibr monocular output files.\n";
      return -1;
    }
  }
  return 0;
}


#include <boost/container_hash/hash.hpp>
#include <cassert>
#include <fstream>
#include <functional>
#include <iostream>
#include <unordered_map>
#include <vector>

#include "AStar.hpp"

// A complete graph with weighted edges
class Graph {
public:
  typedef std::pair<int, int> Edge;
  // Reads graph data from file
  void read_graph(std::ifstream &f) {
    f >> n;        // number of vertices
    f >> p1 >> p2; // endpoints
    for (int i = 0; i < n; ++i) {
      for (int j = 0; j < n; ++j) {
        // Add edge
        f >> weight[{i, j}];
      }
    }
    // length of shortest path as previously calculated
    f >> shortest_dist;
  }
  // Returns the distance from v1 to v2
  float distance(int v1, int v2) { return weight[{v1, v2}]; }
  // Saves the neighbours of vertex v into neighbourhood.
  // Actually the graphs are complete so neighbourhood is always {0,1,...,n-1}.
  void neighbours(std::vector<int> &neighbourhood, int v) {
    neighbourhood.clear();
    for (int i = 0; i < n; ++i) {
      neighbourhood.push_back(i);
    }
  }
  // Graph information as read from the file
  // start and goal vertices
  std::pair<int, int> endpoints() { return {p1, p2}; }
  // Known shortest distance
  float get_shortest_dist() { return shortest_dist; }

private:
  int n;
  int p1, p2; // endpoints
  float shortest_dist;
  std::unordered_map<Edge, float, boost::hash<Edge>> weight;
};

// Wraps around Graph object's methods
auto distance_wrapper(Graph &g) {
  return [&](int v1, int v2) { return g.distance(v1, v2); };
}

auto neighbour_wrapper(Graph &g) {
  return [&](std::vector<int> &neighbourhood, int v) {
    g.neighbours(neighbourhood, v);
  };
}

// Compare to within roundoff error
template <class T> bool close_enough(const T &a, const T &b) {
  return std::abs(a - b) < 1e-3;
}

int main() {
  // Read the test data
  std::ifstream f("test_data.txt");
  Graph g;

  while (!f.eof()) {
    g.read_graph(f);

    AStarSearch<int, float> a;
    a.set_heuristic();
    a.set_distance(distance_wrapper(g));
    a.set_neighbours(neighbour_wrapper(g));

    auto endp = g.endpoints();
    a.set_start(endp.first);
    a.set_goal(endp.second);
    a.solve();

    // For each graph, see whether the algorithm gives the same results
    assert(close_enough(a.get_length(), g.get_shortest_dist()));
  }
  f.close();
  return 0;
}


/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2016 Dimitri Racordon.
/// @author Dimitri Racordon

#include <chrono>
#include <iostream>
#include <boost/range/adaptor/reversed.hpp>
#include <set>

#include "sdd/sdd.hh"
#include "sdd/tools/dot/sdd.hh"
#include "sdd/tools/nodes.hh"

#include "ArchModel.hpp"
#include "Config.hpp"
#include "Filters.hpp"
#include "Schedulers.hpp"
#include "TaskModel.hpp"
#include "TaskModelGenerator.hpp"

using SDD = sdd::SDD<conf>;
using Hom = sdd::homomorphism<conf>;
using Identifier = conf::Identifier;
using Values = conf::Values;

int main(int argc, const char * argv[]) {
    // Initialize sddlib.
    auto manager = sdd::init<conf>();

    // Create the architectur model.
    uint32_t core_number = 1;
    if (argc >= 3) {
        core_number = atoi(argv[2]);
    }
    ArchModel am(core_number);

    // Create a task model.
    uint32_t task_number = 3;
    if (argc >= 2) {
        task_number = atoi(argv[1]);
    }
    TaskModel tm = generate_task_model(task_number);

    // Create the variable order with the core variables first, followed by
    // the task variables.
    sdd::order_builder<conf> ob;

    const auto task_order = tm.build_order();
    for (uint32_t i = (uint32_t)task_order.size(); i > 0; --i) {
        ob.push(task_order[i - 1]);
    }

    for (uint32_t i = 0; i < am.core_number; ++i) {
        ob.push(i | (1 << 31));
    }
    sdd::order<conf> order(ob);

    // Create the initial state.
    SDD m0(order, [](Identifier e){
        if (e & (1 << 31)) {
            // Assign core variables to (0, 0).
            return Values{Arc(0, 0)};
        } else {
            // Assign task variables to (epsilon, 0).
            return Values{Arc::epsilon()};
        }
    });

    auto all_schedulers_set = std::set<Hom>{};

    // Compute all the schedulers for t.
    for (Identifier t = 0; t < tm.task_number; ++t) {
        auto t_schedulers_set = std::set<Hom>{};
        auto filters_set = std::set<Hom>{};

        // We make a filter for the dependencies of t.
        for (auto u : tm.dependencies_of(t)) {
            filters_set.insert(sdd::function<conf>(order, u, filter_constant{Arc::epsilon(), false}));
        }

        // We make a filter for the schedulings where t is already assigned.
        filters_set.insert(sdd::function<conf>(order, t, filter_constant{Arc::epsilon(), true}));

        // The remaining also depends on the core ...
        for (Identifier c = 0; c < am.core_number; ++c) {
            // We make a filter for the timing constraints of t on c.
            filters_set.insert(sdd::function<conf>(order, c | (1 << 31), filter_constraints{t, tm}));

            // We combine all the filters
            const auto filters = sdd::intersection(order, filters_set.begin(), filters_set.end());

            // We make the scheduler for t on c.
            const auto scheduler = sdd::inductive<conf>(schedule{t, c, tm});

            // Finally, the homomorphism for t on c is given by the composition of the
            // filters with the scheduler.
            t_schedulers_set.insert(sdd::composition(scheduler, filters));
        }

        // The homomorphisms for the schedulings of t is given by the union of the
        // schedulers for t on all cores.
        all_schedulers_set.insert(sdd::sum(order, t_schedulers_set.begin(), t_schedulers_set.end()));
    }

    // The final homomorphism is the fixed point on the union of all schedulers for all tasks.
    all_schedulers_set.insert(sdd::id<conf>());
    Hom all_schedulers = sdd::sum(order, all_schedulers_set.begin(), all_schedulers_set.end());
    all_schedulers = sdd::fixpoint(all_schedulers);
    all_schedulers = sdd::rewrite(order, all_schedulers);

    std::chrono::time_point<std::chrono::system_clock> start, end;
    std::size_t elapsed;

    start = std::chrono::system_clock::now();
    SDD schedulings = all_schedulers(order, m0);
    end = std::chrono::system_clock::now();

    // Filter the result to remove the scheduling that are not feasible for all tasks.
    auto feasible_set = std::set<Hom>{};
    for (Identifier t = 0; t < tm.task_number; ++t) {
        feasible_set.insert(sdd::function<conf>(order, t, filter_constant{Arc::epsilon(), false}));
    }
    Hom feasible = sdd::intersection(order, feasible_set.begin(), feasible_set.end());
    feasible = sdd::rewrite(order, feasible);
    schedulings = feasible(order, schedulings);

    elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "Time: " << elapsed << "ms" << std::endl;
    std::cout << "Number of nodes: " << sdd::tools::nodes(schedulings).first << std::endl;
    std::cout << "Number of schedulings: " << schedulings.size() << std::endl;

    return 0;
}


#include <iostream>
#include <vector>
#include <string>
#include "jefflib.h"
#include <boost/tokenizer.hpp>
#include <map>
#include <set>

using namespace std;
using namespace boost;

struct node_t{
    map<int, node_t*> children;
    vector<int> metadata;
    int value = 0;
};

void buildTree(node_t* node, vector<int>& data, int& index);

int main()
{
    vector<string> vect;
    if(GetStringInput(vect)){
        cout << "Got data!" << endl;
        cout << endl;
    }
    else {
        cout << "Failed to read input :( " << endl;
        return -1;
    }

    char_separator<char> sep(" ");
    tokenizer< char_separator<char> > tokens(vect[0], sep);
    vector<string> v_temp(tokens.begin(), tokens.end());

    vector<int> data;
    for(auto s : v_temp){
        int val = stoi(s);
        data.push_back(val);
    }

    int index = 0;

    node_t* root = new node_t();
    index = 0;
    buildTree(root, data, index);
    cout << "Value: " << root->value << endl;
}

void buildTree(node_t* node, vector<int>& data, int& index){
    int numChildren = data[index];
    int numMeta = data[++index];

    for(int i = 0; i < numChildren; i++){
        node_t* tempNode = new node_t{};
        node->children[i+1] = tempNode;
        buildTree(tempNode, data, ++index);
    }

    for(int i = 0; i < numMeta; i++){
        node->metadata.push_back(data[++index]);
    }

    if(numChildren == 0){
        for(int val : node->metadata){
            node->value += val;
        }
    }
    else{
        for(int md : node->metadata){
            if(node->children.find(md) != node->children.end()){
               node->value += node->children[md]->value;
            }
        }
    }
}


#include <boost/geometry/util/rational.hpp>


/*
 *  testOrientation.cpp
 *  Trogdor6
 *
 *  Created by Paul C Hansen on 7/22/11.
 *  Copyright 2011 Stanford University. All rights reserved.
 *
 *  This file is covered by the MIT license.  See LICENSE.txt.
 */

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE testTrackedPolyhedronSensitivity

#include <boost/test/unit_test.hpp>

#include <iostream>
#include <cstdlib>
#include <ctime>
#include "Voxelize.h"
#include "SimpleMesh/Search.h"

using namespace std;

static Matrix3d sFaceMatrix(int xyz)
{
    return outerProduct(Vector3d::unit(xyz), Vector3d::unit(xyz));
}

#define CHECK_CLOSE(mat1, mat2) BOOST_CHECK_SMALL(normInf((mat1)-(mat2)), 1e-10);

const int X = 0, Y = 1, Z = 2;

BOOST_AUTO_TEST_CASE( FaceMatrix )
{
    Matrix3d Mat00 = sFaceMatrix(0);
    Matrix3d Mat00_ref(Matrix3d::zero());
    Mat00_ref(0,0) = 1.0;

    BOOST_CHECK(Mat00 == Mat00_ref);
}

BOOST_AUTO_TEST_CASE( StandardBlock )
{
    Rect3d bounds(-3,-2,-1,3,2,1);
    Rect3i voxels(-1,-1,-1,1,1,1);
    Rect3d cubeBounds(-2, -4.0/3.0, -2.0/3.0, 2, 4.0/3, 2.0/3);

    std::vector<SimpleMesh::ControlVertex> controlVertices1, controlVertices2;
    std::vector<SimpleMesh::Triangle> meshTris1, meshTris2;
    SimpleMesh::Test::oneBoxMesh(controlVertices1, meshTris1, cubeBounds);

    Voxelize vox(bounds, voxels);

    RLE::DynamicRLE3<Matrix3d> ori = vox.orientations(meshTris1);

    // These matrices are the sum of polygon areas times orientation matrices.
    // I had to draw some diagrams to get this straight.  :-)

    Matrix3d cornerOrientation = 2./9*sFaceMatrix(X) +
        1./3*sFaceMatrix(Y) + 2./3*sFaceMatrix(Z);

    Matrix3d xyOrientation = 4./9*sFaceMatrix(X) + 2./3*sFaceMatrix(Y);
    Matrix3d yzOrientation = 2./3*sFaceMatrix(Y) + 4./3*sFaceMatrix(Z);
    Matrix3d zxOrientation = 4./3*sFaceMatrix(Z) + 4./9*sFaceMatrix(X);

    Matrix3d xOrientation = 8./9*sFaceMatrix(X);
    Matrix3d yOrientation = 4./3*sFaceMatrix(Y);
    Matrix3d zOrientation = 8./3*sFaceMatrix(Z);

    Matrix3d zed = Matrix3d::zero();

    for (int xx = -1; xx <= 1; xx += 2)
    for (int yy = -1; yy <= 1; yy += 2)
    for (int zz = -1; zz <= 1; zz += 2)
    {
        CHECK_CLOSE(ori.at(xx,yy,zz), cornerOrientation);
    }

    for (int xx = -1; xx <= 1; xx += 2)
    for (int yy = -1; yy <= 1; yy += 2)
    {
        CHECK_CLOSE(ori.at(xx,yy,0), xyOrientation);
    }

    for (int yy = -1; yy <= 1; yy += 2)
    for (int zz = -1; zz <= 1; zz += 2)
    {
        CHECK_CLOSE(ori.at(0,yy,zz), yzOrientation);
    }

    for (int zz = -1; zz <= 1; zz += 2)
    for (int xx = -1; xx <= 1; xx += 2)
    {
        CHECK_CLOSE(ori.at(xx,0,zz), zxOrientation);
    }

    for (int ww = -1; ww <= 1; ww += 2)
    {
        CHECK_CLOSE(ori.at(ww,0,0), xOrientation);
        CHECK_CLOSE(ori.at(0,ww,0), yOrientation);
        CHECK_CLOSE(ori.at(0,0,ww), zOrientation);
    }

    CHECK_CLOSE(ori.at(0,0,0), zed);
}


// test file for octonion.hpp

//  (C) Copyright Hubert Holin 2001. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

#include <iomanip>
#include <iostream>
#include <sstream>
#include <functional>

#include <boost/bind.hpp>
#include <boost/function.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/test/unit_test_suite_ex.hpp>

#include <boost/math/octonion.hpp>

#ifdef    BOOST_NO_STDC_NAMESPACE
using    ::sqrt;
using    ::atan;
using    ::log;
using    ::exp;
using    ::cos;
using    ::sin;
using    ::tan;
using    ::cosh;
using    ::sinh;
using    ::tanh;
#endif    /* BOOST_NO_STDC_NAMESPACE */

#ifdef    BOOST_NO_ARGUMENT_DEPENDENT_LOOKUP
using    ::boost::math::real;
using    ::boost::math::unreal;
using    ::boost::math::sup;
using    ::boost::math::l1;
using    ::boost::math::abs;
using    ::boost::math::norm;
using    ::boost::math::conj;
using    ::boost::math::exp;
using    ::boost::math::pow;
using    ::boost::math::cos;
using    ::boost::math::sin;
using    ::boost::math::tan;
using    ::boost::math::cosh;
using    ::boost::math::sinh;
using    ::boost::math::tanh;
using    ::boost::math::sinc_pi;
using    ::boost::math::sinhc_pi;
#endif    /* BOOST_NO_ARGUMENT_DEPENDENT_LOOKUP */

// Provide standard floating point abs() overloads for MSVC
#ifdef    BOOST_MSVC
#if        (BOOST_MSVC < 1300) || defined(_MSC_EXTENSIONS)
inline float        abs(float v)
{
    return(fabs(v));
}

inline double        abs(double v)
{
    return(fabs(v));
}

inline long double    abs(long double v)
{
    return(fabs(v));
}
#endif    /* (BOOST_MSVC < 1300) || defined(_MSC_EXTENSIONS) */
#endif    /* BOOST_MSVC */

// explicit (if ludicrous) instanciation
#ifndef __GNUC__
template    class ::boost::math::octonion<int>;
#else
// gcc doesn't like the absolutely-qualified namespace
template class boost::math::octonion<int>;
#endif

namespace
{
    template<typename T>
    ::boost::math::octonion<T>    index_i_element(int idx)
    {
        return(
            ::boost::math::octonion<T>(
                        (idx == 0) ?
                            static_cast<T>(1) :
                            static_cast<T>(0),
                        (idx == 1) ?
                            static_cast<T>(1) :
                            static_cast<T>(0),
                        (idx == 2) ?
                            static_cast<T>(1) :
                            static_cast<T>(0),
                        (idx == 3) ?
                            static_cast<T>(1) :
                            static_cast<T>(0),
                        (idx == 4) ?
                            static_cast<T>(1) :
                            static_cast<T>(0),
                        (idx == 5) ?
                            static_cast<T>(1) :
                            static_cast<T>(0),
                        (idx == 6) ?
                            static_cast<T>(1) :
                            static_cast<T>(0),
                        (idx == 7) ?
                            static_cast<T>(1) :
                            static_cast<T>(0)
            ));
    }
}

void    octonion_manual_test()
{
    // tests for evaluation by humans

    // using default constructor
    ::boost::math::octonion<float>            o0;

    ::boost::math::octonion<float>            oa[2];

    // using constructor "O seen as R^8"
    ::boost::math::octonion<float>            o1(1,2,3,4,5,6,7,8);

    ::std::complex<double>                    c0(9,10);

    // using constructor "O seen as C^4"
    ::boost::math::octonion<double>            o2(c0);

    ::boost::math::quaternion<long double>    q0(11,12,13,14);

    // using constructor "O seen as H^2"
    ::boost::math::octonion<long double>      o3(q0);

    // using UNtemplated copy constructor
    ::boost::math::octonion<float>            o4(o1);

    // using templated copy constructor
    ::boost::math::octonion<long double>      o5(o2);

    // using UNtemplated assignment operator
    o5 = o3;
    oa[0] = o0;

    // using templated assignment operator
    o5 = o2;
    oa[1] = o5;

    float                                     f0(15);

    // using converting assignment operator
    o0 = f0;

    // using converting assignment operator
    o2 = c0;

    // using converting assignment operator
    o5 = q0;

    // using += (const T &)
    o4 += f0;

    // using += (const ::std::complex<T> &)
    o2 += c0;

    // using += (const ::boost::math::quaternion<T> &)
    o3 += q0;

    // using += (const quaternion<X> &)
    o5 += o4;

    // using -= (const T &)
    o1 -= f0;

    // using -= (const ::std::complex<T> &)
    o2 -= c0;

    // using -= (const ::boost::math::quaternion<T> &)
    o5 -= q0;

    // using -= (const octonion<X> &)
    o3 -= o4;

    double                                    d0(16);
    ::std::complex<double>                    c1(17,18);
    ::boost::math::quaternion<double>         q1(19,20,21,22);

    // using *= (const T &)
    o2 *= d0;

    // using *= (const ::std::complex<T> &)
    o2 *= c1;

    // using *= (const ::boost::math::quaternion<T> &)
    o2 *= q1;

    // using *= (const octonion<X> &)
    o2 *= o4;

    long double                               l0(23);
    ::std::complex<long double>               c2(24,25);

    // using /= (const T &)
    o5 /= l0;

    // using /= (const ::std::complex<T> &)
    o5 /= c2;

    // using /= (const quaternion<X> &)
    o5 /= q0;

    // using /= (const octonion<X> &)
    o5 /= o5;

    // using + (const T &, const octonion<T> &)
    ::boost::math::octonion<float>            o6 = f0+o0;

    // using + (const octonion<T> &, const T &)
    ::boost::math::octonion<float>            o7 = o0+f0;

    // using + (const ::std::complex<T> &, const quaternion<T> &)
    ::boost::math::octonion<double>           o8 = c0+o2;

    // using + (const octonion<T> &, const ::std::complex<T> &)
    ::boost::math::octonion<double>           o9 = o2+c0;

    // using + (const ::boost::math::quaternion<T>, const octonion<T> &)
    ::boost::math::octonion<long double>      o10 = q0+o3;

    // using + (const octonion<T> &, const ::boost::math::quaternion<T> &)
    ::boost::math::octonion<long double>      o11 = o3+q0;

    // using + (const quaternion<T> &,const quaternion<T> &)
    ::boost::math::octonion<float>            o12 = o0+o4;

    // using - (const T &, const octonion<T> &)
    o6 = f0-o0;

    // using - (const octonion<T> &, const T &)
    o7 = o0-f0;

    // using - (const ::std::complex<T> &, const octonion<T> &)
    o8 = c0-o2;

    // using - (const octonion<T> &, const ::std::complex<T> &)
    o9 = o2-c0;

    // using - (const quaternion<T> &,const octonion<T> &)
    o10 = q0-o3;

    // using - (const octonion<T> &,const quaternion<T> &)
    o11 = o3-q0;

    // using - (const octonion<T> &,const octonion<T> &)
    o12 = o0-o4;

    // using * (const T &, const octonion<T> &)
    o6 = f0*o0;

    // using * (const octonion<T> &, const T &)
    o7 = o0*f0;

    // using * (const ::std::complex<T> &, const octonion<T> &)
    o8 = c0*o2;

    // using * (const octonion<T> &, const ::std::complex<T> &)
    o9 = o2*c0;

    // using * (const quaternion<T> &,const octonion<T> &)
    o10 = q0*o3;

    // using * (const octonion<T> &,const quaternion<T> &)
    o11 = o3*q0;

    // using * (const octonion<T> &,const octonion<T> &)
    o12 = o0*o4;

    // using / (const T &, const octonion<T> &)
    o6 = f0/o0;

    // using / (const octonion<T> &, const T &)
    o7 = o0/f0;

    // using / (const ::std::complex<T> &, const octonion<T> &)
    o8 = c0/o2;

    // using / (const octonion<T> &, const ::std::complex<T> &)
    o9 = o2/c0;

    // using / (const ::boost::math::quaternion<T> &, const octonion<T> &)
    o10 = q0/o3;

    // using / (const octonion<T> &, const ::boost::math::quaternion<T> &)
    o11 = o3/q0;

    // using / (const octonion<T> &,const octonion<T> &)
    o12 = o0/o4;

    // using + (const octonion<T> &)
    o4 = +o0;

    // using - (const octonion<T> &)
    o0 = -o4;

    // using == (const T &, const octonion<T> &)
    f0 == o0;

    // using == (const octonion<T> &, const T &)
    o0 == f0;

    // using == (const ::std::complex<T> &, const octonion<T> &)
    c0 == o2;

    // using == (const octonion<T> &, const ::std::complex<T> &)
    o2 == c0;

    // using == (const ::boost::math::quaternion<T> &, const octonion<T> &)
    q0 == o3;

    // using == (const octonion<T> &, const ::boost::math::quaternion<T> &)
    o3 == q0;

    // using == (const octonion<T> &,const octonion<T> &)
    o0 == o4;

    // using != (const T &, const octonion<T> &)
    f0 != o0;

    // using != (const octonion<T> &, const T &)
    o0 != f0;

    // using != (const ::std::complex<T> &, const octonion<T> &)
    c0 != o2;

    // using != (const octonion<T> &, const ::std::complex<T> &)
    o2 != c0;

    // using != (const ::boost::math::quaternion<T> &, const octonion<T> &)
    q0 != o3;

    // using != (const octonion<T> &, const ::boost::math::quaternion<T> &)
    o3 != q0;

    // using != (const octonion<T> &,const octonion<T> &)
    o0 != o4;

    BOOST_MESSAGE("Please input an octonion...");

#ifdef BOOST_INTERACTIVE_TEST_INPUT_ITERATOR
    ::std::cin >> o0;

    if    (::std::cin.fail())
    {
        BOOST_MESSAGE("You have entered nonsense!");
    }
    else
    {
        BOOST_MESSAGE("You have entered the octonion " << o0 << " .");
    }
#else
    ::std::istringstream                bogus("(1,2,3,4,5,6,7,8)");

    bogus >> o0;

    BOOST_MESSAGE("You have entered the octonion " << o0 << " .");
#endif

    BOOST_MESSAGE("For this octonion:");

    BOOST_MESSAGE( "the value of the real part is "
                << real(o0));

    BOOST_MESSAGE( "the value of the unreal part is "
                << unreal(o0));

    BOOST_MESSAGE( "the value of the sup norm is "
                << sup(o0));

    BOOST_MESSAGE( "the value of the l1 norm is "
                << l1(o0));

    BOOST_MESSAGE( "the value of the magnitude (euclidian norm) is "
                << abs(o0));

    BOOST_MESSAGE( "the value of the (Cayley) norm is "
                << norm(o0));

    BOOST_MESSAGE( "the value of the conjugate is "
                << conj(o0));

    BOOST_MESSAGE( "the value of the exponential is "
                << exp(o0));

    BOOST_MESSAGE( "the value of the cube is "
                << pow(o0,3));

    BOOST_MESSAGE( "the value of the cosinus is "
                << cos(o0));

    BOOST_MESSAGE( "the value of the sinus is "
                << sin(o0));

    BOOST_MESSAGE( "the value of the tangent is "
                << tan(o0));

    BOOST_MESSAGE( "the value of the hyperbolic cosinus is "
                << cosh(o0));

    BOOST_MESSAGE( "the value of the hyperbolic sinus is "
                << sinh(o0));

    BOOST_MESSAGE( "the value of the hyperbolic tangent is "
                << tanh(o0));

#ifdef    BOOST_NO_TEMPLATE_TEMPLATES
    BOOST_MESSAGE( "no template templates, can't compute cardinal functions");
#else    /* BOOST_NO_TEMPLATE_TEMPLATES */
    BOOST_MESSAGE( "the value of the Sinus Cardinal (of index pi) is "
                << sinc_pi(o0));

    BOOST_MESSAGE( "the value of "
                << "the Hyperbolic Sinus Cardinal (of index pi) is "
                << sinhc_pi(o0));
#endif    /* BOOST_NO_TEMPLATE_TEMPLATES */

    BOOST_MESSAGE(" ");

    float                            rho = ::std::sqrt(4096.0f);
    float                            theta = ::std::atan(1.0f);
    float                            phi1 = ::std::atan(1.0f);
    float                            phi2 = ::std::atan(1.0f);
    float                            phi3 = ::std::atan(1.0f);
    float                            phi4 = ::std::atan(1.0f);
    float                            phi5 = ::std::atan(1.0f);
    float                            phi6 = ::std::atan(1.0f);

    BOOST_MESSAGE( "The value of the octonion represented "
                << "in spherical form by "
                << "rho = " << rho << " , theta = " << theta
                << " , phi1 = " << phi1 << " , phi2 = " << phi2
                << " , phi3 = " << phi3 << " , phi4 = " << phi4
                << " , phi5 = " << phi5 << " , phi6 = " << phi6
                << " is "
                << ::boost::math::spherical(rho, theta,
                        phi1, phi2, phi3, phi4, phi5, phi6));

    float                            rho1 = 1;
    float                            rho2 = 2;
    float                            rho3 = ::std::sqrt(2.0f);
    float                            rho4 = ::std::sqrt(8.0f);
    float                            theta1 = 0;
    float                            theta2 = ::std::atan(1.0f)*2;
    float                            theta3 = ::std::atan(1.0f);
    float                            theta4 = ::std::atan(::std::sqrt(3.0f));

    BOOST_MESSAGE( "The value of the octonion represented "
                << "in multipolar form by "
                << "rho1 = " << rho1 << " , theta1 = " << theta1
                << " , rho2 = " << rho2 << " , theta2 = " << theta2
                << "rho3 = " << rho3 << " , theta3 = " << theta3
                << " , rho4 = " << rho4 << " , theta4 = " << theta4
                << " is "
                << ::boost::math::multipolar(rho1, theta1, rho2, theta2,
                        rho3, theta3, rho4, theta4));

    float                            r = ::std::sqrt(2.0f);
    float                            angle = ::std::atan(1.0f);
    float                            h1 = 3;
    float                            h2 = 4;
    float                            h3 = 5;
    float                            h4 = 6;
    float                            h5 = 7;
    float                            h6 = 8;

    BOOST_MESSAGE( "The value of the octonion represented "
                << "in cylindrical form by "
                << "r = " << r << " , angle = " << angle
                << " , h1 = " << h1 << " , h2 = " << h2
                << " , h3 = " << h3 << " , h4 = " << h4
                << " , h5 = " << h5 << " , h6 = " << h6
                << " is " << ::boost::math::cylindrical(r, angle,
                        h1, h2, h3, h4, h5, h6));

    double                               real_1(1);
    ::std::complex<double>               complex_1(1);
    ::std::complex<double>               complex_i(0,1);
    ::boost::math::quaternion<double>    quaternion_1(1);
    ::boost::math::quaternion<double>    quaternion_i(0,1);
    ::boost::math::quaternion<double>    quaternion_j(0,0,1);
    ::boost::math::quaternion<double>    quaternion_k(0,0,0,1);
    ::boost::math::octonion<double>      octonion_1(1);
    ::boost::math::octonion<double>      octonion_i(0,1);
    ::boost::math::octonion<double>      octonion_j(0,0,1);
    ::boost::math::octonion<double>      octonion_k(0,0,0,1);
    ::boost::math::octonion<double>      octonion_e_prime(0,0,0,0,1);
    ::boost::math::octonion<double>      octonion_i_prime(0,0,0,0,0,1);
    ::boost::math::octonion<double>      octonion_j_prime(0,0,0,0,0,0,1);
    ::boost::math::octonion<double>      octonion_k_prime(0,0,0,0,0,0,0,1);

    BOOST_MESSAGE(" ");

    BOOST_MESSAGE( "Real 1: " << real_1
                << " ; Complex 1: " << complex_1
                << " ; Quaternion 1: " << quaternion_1
                << " ; Octonion 1: " << octonion_1 << " .");

    BOOST_MESSAGE( "Complex i: " << complex_i
                << " ; Quaternion i: " << quaternion_i
                << " ; Octonion i : " << octonion_i << " .");

    BOOST_MESSAGE( "Quaternion j: " << quaternion_j
                << " ; Octonion j: " << octonion_j << " .");

    BOOST_MESSAGE( "Quaternion k: " << quaternion_k
                << " ; Octonion k: " << octonion_k << " .");

    BOOST_MESSAGE( "Quaternion e\': " << octonion_e_prime << " .");

    BOOST_MESSAGE( "Quaternion i\': " << octonion_i_prime << " .");

    BOOST_MESSAGE( "Quaternion j\': " << octonion_j_prime << " .");

    BOOST_MESSAGE( "Quaternion k\': " << octonion_k_prime << " .");

    BOOST_MESSAGE(" ");

    BOOST_MESSAGE( octonion_1*octonion_1 << " ; "
                << octonion_1*octonion_i << " ; "
                << octonion_1*octonion_j << " ; "
                << octonion_1*octonion_k << " ; "
                << octonion_1*octonion_e_prime << " ; "
                << octonion_1*octonion_i_prime << " ; "
                << octonion_1*octonion_j_prime << " ; "
                << octonion_1*octonion_k_prime << " ; ");

    BOOST_MESSAGE( octonion_i*octonion_1 << " ; "
                << octonion_i*octonion_i << " ; "
                << octonion_i*octonion_j << " ; "
                << octonion_i*octonion_k << " ; "
                << octonion_i*octonion_e_prime << " ; "
                << octonion_i*octonion_i_prime << " ; "
                << octonion_i*octonion_j_prime << " ; "
                << octonion_i*octonion_k_prime << " ; ");

    BOOST_MESSAGE( octonion_j*octonion_1 << " ; "
                << octonion_j*octonion_i << " ; "
                << octonion_j*octonion_j << " ; "
                << octonion_j*octonion_k << " ; "
                << octonion_j*octonion_e_prime << " ; "
                << octonion_j*octonion_i_prime << " ; "
                << octonion_j*octonion_j_prime << " ; "
                << octonion_j*octonion_k_prime << " ; ");

    BOOST_MESSAGE( octonion_k*octonion_1 << " ; "
                << octonion_k*octonion_i << " ; "
                << octonion_k*octonion_j << " ; "
                << octonion_k*octonion_k << " ; "
                << octonion_k*octonion_e_prime << " ; "
                << octonion_k*octonion_i_prime << " ; "
                << octonion_k*octonion_j_prime << " ; "
                << octonion_k*octonion_k_prime << " ; ");

    BOOST_MESSAGE( octonion_e_prime*octonion_1 << " ; "
                << octonion_e_prime*octonion_i << " ; "
                << octonion_e_prime*octonion_j << " ; "
                << octonion_e_prime*octonion_k << " ; "
                << octonion_e_prime*octonion_e_prime << " ; "
                << octonion_e_prime*octonion_i_prime << " ; "
                << octonion_e_prime*octonion_j_prime << " ; "
                << octonion_e_prime*octonion_k_prime << " ; ");

    BOOST_MESSAGE( octonion_i_prime*octonion_1 << " ; "
                << octonion_i_prime*octonion_i << " ; "
                << octonion_i_prime*octonion_j << " ; "
                << octonion_i_prime*octonion_k << " ; "
                << octonion_i_prime*octonion_e_prime << " ; "
                << octonion_i_prime*octonion_i_prime << " ; "
                << octonion_i_prime*octonion_j_prime << " ; "
                << octonion_i_prime*octonion_k_prime << " ; ");

    BOOST_MESSAGE( octonion_j_prime*octonion_1 << " ; "
                << octonion_j_prime*octonion_i << " ; "
                << octonion_j_prime*octonion_j << " ; "
                << octonion_j_prime*octonion_k << " ; "
                << octonion_j_prime*octonion_e_prime << " ; "
                << octonion_j_prime*octonion_i_prime << " ; "
                << octonion_j_prime*octonion_j_prime << " ; "
                << octonion_j_prime*octonion_k_prime << " ; ");

    BOOST_MESSAGE( octonion_k_prime*octonion_1 << " ; "
                << octonion_k_prime*octonion_i << " ; "
                << octonion_k_prime*octonion_j << " ; "
                << octonion_k_prime*octonion_k << " ; "
                << octonion_k_prime*octonion_e_prime << " ; "
                << octonion_k_prime*octonion_i_prime << " ; "
                << octonion_k_prime*octonion_j_prime << " ; "
                << octonion_k_prime*octonion_k_prime << " ; ");

    BOOST_MESSAGE(" ");

    BOOST_MESSAGE("i\'*(e\'*j) : "
    << octonion_i_prime*(octonion_e_prime*octonion_j) << " ;");

    BOOST_MESSAGE("(i\'*e\')*j : "
    << (octonion_i_prime*octonion_e_prime)*octonion_j << " ;");

    BOOST_MESSAGE(" ");
}

template<typename T>
void    multiplication_test(const char * more_blurb)
{
    using ::std::numeric_limits;

    using ::boost::math::abs;

    BOOST_MESSAGE("Testing multiplication for " << more_blurb << ".");

    BOOST_REQUIRE_PREDICATE(::std::less_equal<T>(), 2,
        (
            abs(::boost::math::octonion<T>(1,0,0,0,0,0,0,0)*
                ::boost::math::octonion<T>(1,0,0,0,0,0,0,0)-
                static_cast<T>(1)),
            numeric_limits<T>::epsilon()
        ));

    for    (int idx = 1; idx < 8; ++idx)
    {
        ::boost::math::octonion<T>    toto = index_i_element<T>(idx);

        BOOST_REQUIRE_PREDICATE(::std::less_equal<T>(), 2,
            (
                abs(toto*toto+static_cast<T>(1)),
                numeric_limits<T>::epsilon()
            ));
    }
}

template<typename T>
void    exp_test(const char * more_blurb)
{
    using ::std::numeric_limits;

    using ::std::atan;

    using ::boost::math::abs;

    BOOST_MESSAGE("Testing exp for " << more_blurb << ".");

    for    (int idx = 1; idx < 8; ++idx)
    {
        ::boost::math::octonion<T>    toto =
            static_cast<T>(4)*atan(static_cast<T>(1))*index_i_element<T>(idx);

        BOOST_CHECK_PREDICATE(::std::less_equal<T>(), 2,
            (
                abs(exp(toto)+static_cast<T>(1)),
                2*numeric_limits<T>::epsilon()
            ));
    }
}

boost::unit_test_framework::test_suite *    init_unit_test_suite(int, char *[])
{
    //::boost::unit_test_framework::unit_test_log::instance().
    //    set_log_threshold_level_by_name("messages");

    boost::unit_test_framework::test_suite *    test =
        BOOST_TEST_SUITE("octonion_test");

#define    BOOST_OCTONION_COMMON_GENERATOR(fct,type)               \
    test->add(BOOST_TEST_CASE(::boost::bind(static_cast            \
        < void (*) (const char *) >(&fct##_test<type>), #type)));

#define    BOOST_OCTONION_TEST(type)                        \
    BOOST_OCTONION_COMMON_GENERATOR(multiplication,type)    \
    BOOST_OCTONION_COMMON_GENERATOR(exp,type)

    BOOST_OCTONION_TEST(float)
    BOOST_OCTONION_TEST(double)
    BOOST_OCTONION_TEST(long double)

#undef    BOOST_OCTONION_TEST

#undef    BOOST_OCTONION_COMMON_GENERATOR

#ifdef BOOST_OCTONION_TEST_VERBOSE

    octonion_manual_test();

#endif    /* BOOST_OCTONION_TEST_VERBOSE */

    return(test);
}


//==================================================================================================
/*!
  @file

    @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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DIV_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DIV_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-operator
    This function object computes the quotient of the two parameters of the
    same type with or without option.

    @par Header <boost/simd/function/div.hpp>

    @par Notes

    For any value @c x and @c y of same type `div({option, }x, y)`
    returns the quotient of @c x by @c y respecting to the option specified.

    With no option this function is equivalent to `divides(x, y)` (See @ref divides.)

    Options may be @ref ceil, @ref floor, @ref fix, @ref round, @ref nearbyint (in the
    namespace booost::simd) and provides the same result as if the function object whose
    option is the name was applied to the floating division of the parameters.

    @see divides, rec

    @par Example:

      @snippet div.cpp div

    @par Possible output:

      @snippet div.txt div

  **/

  //@{
  Value div(Option const&, Value const& x, Value const& y);
  Value div(Value const& x, Value const& y);
  //@}

} }
#endif

#include <boost/simd/function/scalar/div.hpp>
#include <boost/simd/function/simd/div.hpp>

#endif


#include <gmpxx.h>
#include <boost/test/minimal.hpp>
#include <boost/numeric/mtl/mtl.hpp>

// Check only if malloc error happens as reported by Hui Li

int test_main(int argc, char** argv)
{
    mtl::dense2D<mpq_class> A(2, 2);
    A(0, 0) = mpq_class(0);
    A(0, 1) = 0; // problem
    mpq_class x;
    x = 0;
    return 0;
}

// g++ -g -DMTL_ASSERT_FOR_THROW -I$MTL_BOOST_ROOT -I/home/pgottsch/Download/gmp-4.2.2 -I/home/pgottsch/projects/boost/boost_1_33_1 -o gmp_test gmp_test.cpp -L/home/pgottsch/Download/gmp-4.2.2 -lgmp


/**
 * sudoku_bhc.cpp
 *
 * By Sebastian Raaphorst, 2018.
 */

#include <iostream>
#include <memory>
#include <stdexcept>

#include <boost/program_options.hpp>

#include <GenSudokuBoard.h>
#include <GenSudokuBoardBHCPopulator.h>
#include <HillClimbingAlgorithm.h>

#include "progopts.h"

namespace po = boost::program_options;
using namespace vorpal::gensudoku;
using namespace vorpal::stochastic;

int main(int argc, const char * const argv[]) {
    using solver = HillClimbingAlgorithm<SudokuBoard>;
    solver::options_type options;

    try {
        auto desc = init_options(options);
        auto done = end_options<SudokuBoardBHCPopulator, solver::options_type>(argc, argv, desc, options);
        if (done) return 0;
    } catch (const std::exception &e) {
        std::cerr << e.what() << '\n';
        return 1;
    }

    run<solver>(options);
}


#include <ancse/dg_rate_of_change.hpp>

#include <Eigen/Dense>
#include <ancse/config.hpp>
#include <ancse/polynomial_basis.hpp>
#include <ancse/dg_handler.hpp>
#include <ancse/numerical_flux.hpp>
#include <fmt/format.h>

/// DG numerical flux term
template <class NumericalFlux>
void DGRateOfChange<NumericalFlux>
:: eval_numerical_flux (Eigen::MatrixXd &dudt,
                        const Eigen::MatrixXd &u0) const
{
    // implement the loop for DG numerical flux term.
}

/// DG volume integral term
template <class NumericalFlux>
void DGRateOfChange<NumericalFlux>
:: eval_volume_integral(Eigen::MatrixXd &dudt,
                        const Eigen::MatrixXd &u0) const
{
    // implement the loop for DG volume integral.
}

#define REGISTER_NUMERICAL_FLUX(token, FluxType, flux)          \
    if (config["flux"] == (token)) {                            \
        return std::make_shared< DGRateOfChange<FluxType> >(    \
            grid, model, flux, poly_basis, dg_handler);                     \
    }

std::shared_ptr<RateOfChange> make_dg_rate_of_change(
    const nlohmann::json &config,
    const Grid &grid,
    const std::shared_ptr<Model> &model,
    const PolynomialBasis &poly_basis,
    const DGHandler &dg_handler,
    const std::shared_ptr<SimulationTime> &simulation_time)
{
    // Register the other numerical fluxes.

    throw std::runtime_error(
        fmt::format("Unknown numerical flux. {}",
                    std::string(config["flux"])));
}

#undef REGISTER_NUMERICAL_FLUX