// (C) Copyright Andrew Sutton 2007
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

//[inclusive_mean_geodesic_example
#include <iostream>
#include <iomanip>

#include <boost/graph/directed_graph.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/geodesic_distance.hpp>
#include "helper.hpp"

using namespace std;
using namespace boost;

// This template structure defines the function that we will apply
// to compute both the per-vertex mean geodesic distances and the
// graph's mean geodesic distance.
template < typename Graph, typename DistanceType, typename ResultType,
    typename Divides = divides< ResultType > >
struct inclusive_average
{
    typedef DistanceType distance_type;
    typedef ResultType result_type;

    result_type operator()(distance_type d, const Graph& g)
    {
        if (d == numeric_values< distance_type >::infinity())
        {
            return numeric_values< result_type >::infinity();
        }
        else
        {
            return div(result_type(d), result_type(num_vertices(g)));
        }
    }
    Divides div;
};

// The Page type stores the name of each vertex in the graph and
// represents web pages that can be navigated to.
struct WebPage
{
    string name;
};

// The Link type stores an associated probability of traveling
// from one page to another.
struct Link
{
    float probability;
};

// Declare the graph type and its vertex and edge types.
typedef directed_graph< WebPage, Link > Graph;
typedef graph_traits< Graph >::vertex_descriptor Vertex;
typedef graph_traits< Graph >::edge_descriptor Edge;

// The name map provides an abstract accessor for the names of
// each vertex. This is used during graph creation.
typedef property_map< Graph, string WebPage::* >::type NameMap;

// Declare a matrix type and its corresponding property map that
// will contain the distances between each pair of vertices.
typedef exterior_vertex_property< Graph, float > DistanceProperty;
typedef DistanceProperty::matrix_type DistanceMatrix;
typedef DistanceProperty::matrix_map_type DistanceMatrixMap;

// Declare the weight map as an accessor into the bundled
// edge property.
typedef property_map< Graph, float Link::* >::type WeightMap;

// Declare a container and its corresponding property map that
// will contain the resulting mean geodesic distances of each
// vertex in the graph.
typedef exterior_vertex_property< Graph, float > GeodesicProperty;
typedef GeodesicProperty::container_type GeodesicContainer;
typedef GeodesicProperty::map_type GeodesicMap;

static float exclusive_geodesics(const Graph&, DistanceMatrixMap, GeodesicMap);
static float inclusive_geodesics(const Graph&, DistanceMatrixMap, GeodesicMap);

int main(int argc, char* argv[])
{
    // Create the graph, a name map that providse abstract access
    // to the web page names, and the weight map as an accessor to
    // the edge weights (or probabilities).
    Graph g;
    NameMap nm(get(&WebPage::name, g));
    WeightMap wm(get(&Link::probability, g));

    // Read the weighted graph from standard input.
    read_weighted_graph(g, nm, wm, cin);

    // Compute the distances between all pairs of vertices using
    // the Floyd-Warshall algorithm. The weight map was created
    // above so it could be populated when the graph was read in.
    DistanceMatrix distances(num_vertices(g));
    DistanceMatrixMap dm(distances, g);
    floyd_warshall_all_pairs_shortest_paths(g, dm, weight_map(wm));

    // Create the containers and the respective property maps that
    // will contain the mean geodesics averaged both including
    // self-loop distances and excluding them.
    GeodesicContainer exclude(num_vertices(g));
    GeodesicContainer include(num_vertices(g));
    GeodesicMap exmap(exclude, g);
    GeodesicMap inmap(include, g);

    float ex = exclusive_geodesics(g, dm, exmap);
    float in = inclusive_geodesics(g, dm, inmap);

    // Print the mean geodesic distance of each vertex and finally,
    // the graph itself.
    cout << setw(12) << setiosflags(ios::left) << "vertex";
    cout << setw(12) << setiosflags(ios::left) << "excluding";
    cout << setw(12) << setiosflags(ios::left) << "including" << endl;
    graph_traits< Graph >::vertex_iterator i, end;
    for (boost::tie(i, end) = vertices(g); i != end; ++i)
    {
        cout << setw(12) << setiosflags(ios::left) << g[*i].name << setw(12)
             << get(exmap, *i) << setw(12) << get(inmap, *i) << endl;
    }
    cout << "small world (excluding self-loops): " << ex << endl;
    cout << "small world (including self-loops): " << in << endl;

    return 0;
}

float exclusive_geodesics(const Graph& g, DistanceMatrixMap dm, GeodesicMap gm)
{
    // Compute the mean geodesic distances, which excludes distances
    // of self-loops by default. Return the measure for the entire graph.
    return all_mean_geodesics(g, dm, gm);
}

float inclusive_geodesics(const Graph& g, DistanceMatrixMap dm, GeodesicMap gm)
{
    // Create a new measure object for computing the mean geodesic
    // distance of all vertices. This measure will actually be used
    // for both averages.
    inclusive_average< Graph, float, float > m;

    // Compute the mean geodesic distance using the inclusive average
    // to account for self-loop distances. Return the measure for the
    // entire graph.
    return all_mean_geodesics(g, dm, gm, m);
}
//]


#ifndef BAOBZI_TEMPLATE_HPP
#define BAOBZI_TEMPLATE_HPP

#include <fstream>
#include <iostream>
#include <mutex>
#include <queue>
#include <vector>

#include <msgpack.hpp>
#define EIGEN_MATRIX_PLUGIN "baobzi/eigen_matrix_plugin.h"

#define EIGEN_MAX_ALIGN_BYTES 64
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <unsupported/Eigen/CXX11/Tensor>

#include <baobzi/header.h>

/// Namespace for baobzi
namespace baobzi {

template <int DIM, int ORDER, int ISET>
class Function;

/// @brief Structure to represent geometric portion of Baobzi nodes
/// @tparam D number of dimensions of box
/// @tparam ISET Instruction set index (dummy variable to force alignment for different instruction sets)
template <int D, int ISET>
struct Box {
    using VEC = Eigen::Vector<double, D>;
    VEC center;      ///< Center of box
    VEC half_length; ///< Half the dimension of the box
    VEC inv_half_length; ///< 1.0 / half the dimension of the box

    Box<D, ISET>() = default; ///< default constructor for msgpack happiness
    /// @brief Constructor, just copies x, hl over
    Box<D, ISET>(const VEC &x, const VEC &hl)
        : center(x), half_length(hl), inv_half_length(VEC::Ones().array() / hl.array()) {}

    /// @brief Check if point lies inside box
    /// @param[in] x point to check
    /// @returns true if point in box, false otherwise
    bool contains(const VEC &x) const {
        VEC dx = (x - center).array().abs();
        return !((dx > half_length).any());
    }

    /// @brief MSGPACK serialization magic
    MSGPACK_DEFINE(center, half_length, inv_half_length);
};

/// @brief Return an estimate of the error for a given set of coefficientsj
/// @param[in] coeffs one or two dimensional Vector/Matrix of coefficients
/// @returns estimation of error given those coefficients
inline double standard_error(const Eigen::Ref<Eigen::MatrixXd> &coeffs) {
    double maxcoeff = 0.0;
    double scaling_factor = 1.0;
    if (coeffs.cols() == 1) {
        int n = coeffs.size();
        for (auto i = n - 2; i < n; ++i)
            maxcoeff = std::max(std::abs(coeffs(i, 0)), maxcoeff);
        scaling_factor = std::max(scaling_factor, std::abs(coeffs(0, 0)));
    } else {
        int n = coeffs.rows();
        for (auto i = 0; i < n; ++i)
            maxcoeff = std::max(std::abs(coeffs(i, n - i - 1)), maxcoeff);

        scaling_factor = std::max(scaling_factor, std::abs(coeffs(n - 1, 0)));
        scaling_factor = std::max(scaling_factor, std::abs(coeffs(0, n - 1)));
    }

    return maxcoeff / scaling_factor;
}

/// @brief Evaluate chebyshev polynomial given a box and a point inside that box
/// @tparam DIM dim of chebyshev polynomial to evaluate
/// @tparam ORDER order of chebyshev polynomial to evaluate
/// @tparam ISET Instruction set index (dummy variable to force alignment for different instruction sets)
/// @param[in] x position of point to evaluate
/// @param[in] box box that x lives in
/// @param[in] coeffs_raw flat column-major vector of coefficients
/// @returns value of interpolating function at x
template <int DIM, int ORDER, int ISET>
inline double cheb_eval(const Eigen::Vector<double, DIM> &x, const Box<DIM, ISET> &box,
                        const std::vector<double, Eigen::aligned_allocator<double>> &coeffs_raw);

template <int ORDER, int ISET>
inline double cheb_eval(const Eigen::Vector<double, 1> &x, const Box<1, ISET> &box,
                        const std::vector<double, Eigen::aligned_allocator<double>> &coeffs_raw) {
    double xd = (x[0] - box.center[0]) * box.inv_half_length[0];

    Eigen::Vector<double, ORDER> Tn;
    Tn[0] = 1.0;
    Tn[1] = xd;
    xd *= 2.0;
    for (int i = 2; i < ORDER; ++i)
        Tn[i] = xd * Tn[i - 1] - Tn[i - 2];

    Eigen::Map<const Eigen::Vector<double, ORDER>> coeffs(coeffs_raw.data());

    return coeffs.dot(Tn);
}

template <int ORDER, int ISET>
inline double cheb_eval(const Eigen::Vector2d &x, const Box<2, ISET> &box,
                        const std::vector<double, Eigen::aligned_allocator<double>> &coeffs_raw) {
    Eigen::Vector2d xinterp = (x - box.center).array() * box.inv_half_length.array();
    Eigen::Matrix<double, 2, ORDER> Tns;
    Tns.col(0).setOnes();
    Tns.col(1) = xinterp;
    xinterp *= 2.0;
    for (int i = 2; i < ORDER; ++i)
        Tns.col(i) = xinterp.array() * Tns.col(i - 1).array() - Tns.col(i - 2).array();

    Eigen::Map<const Eigen::Matrix<double, ORDER, ORDER>> coeffs(coeffs_raw.data());

    return Tns.row(0).transpose().dot(coeffs * Tns.row(1).transpose());
}

template <int ORDER, int ISET>
inline double cheb_eval(const Eigen::Vector3d &x, const Box<3, ISET> &box,
                        const std::vector<double, Eigen::aligned_allocator<double>> &coeffs_raw) {

    Eigen::Vector3d xinterp = (x - box.center).array() * box.inv_half_length.array();

    Eigen::Vector<double, ORDER> Tn[3];
    Tn[0][0] = Tn[1][0] = Tn[2][0] = 1.0;
    for (int i = 0; i < 3; ++i) {
        Tn[i][1] = xinterp[i];
        xinterp[i] *= 2.0;
        for (int j = 2; j < ORDER; ++j)
            Tn[i][j] = xinterp[i] * Tn[i][j - 1] - Tn[i][j - 2];
    }

    double res = 0.0;
    using map_t = Eigen::Map<const Eigen::Matrix<double, ORDER, ORDER>>;
    for (int i = 0; i < ORDER; ++i)
        res += Tn[0][i] * Tn[1].dot(map_t(coeffs_raw.data() + i * ORDER * ORDER) * Tn[2]);

    return res;
}

/// @brief Node in baobzi::FunctionTree. If leaf, contains evaluation data, otherwise children
/// @tparam D dimension of function
/// @tparam ORDER order of evaluation polynomial
/// @tparam ISET instruction set index (dummy variable to force alignment for different instruction sets)
template <int D, int ORDER, int ISET>
class Node {
  public:
    using VEC = Eigen::Vector<double, D>; ///< D dimensional vector type
    using CoeffVec = Eigen::Vector<double, ORDER>; ///< ORDER dimensional vector type
    std::vector<double, Eigen::aligned_allocator<double>> coeffs_; ///< Flattened chebyshev coeffs
    using Func = Function<D, ORDER, ISET>; ///< Type of boabzi function this belongs to
    Box<D, ISET> box_;             ///< Geometric position/size of this node
    uint64_t first_child_idx = -1; ///< First child's index in a flattened list of all nodes
    bool leaf_ = false;            ///< Helper variable to determine if node is a leaf

    Node<D, ORDER, ISET>() = default; ///< Default constructor for msgpack happiness

    /// @brief Construct node from box (without fitting)
    /// @param [in] box box this node represents
    Node<D, ORDER, ISET>(const Box<D, ISET> &box) : box_(box) {}

    /// @brief check if node is leaf
    /// @return true if leaf, false otherwise
    inline bool is_leaf() const { return leaf_; }

    /// @brief Fit node to a given tolerance. If fit succeeds, set leaf and coeffs, otherwise ... don't
    ///
    /// Modifies: Node::leaf_, Node::coeffs_
    /// @param[in] input parameters for fit (function, tol, etc)
    /// @returns true if fit successful, false if not good enough
    bool fit(const baobzi_input_t *input) {
        if constexpr (D == 1) {
            Eigen::Vector<double, ORDER> F;
            CoeffVec xvec =
                Func::get_cheb_nodes(box_.center[0] - box_.half_length[0], box_.center[0] + box_.half_length[0]);

            for (int i = 0; i < ORDER; ++i)
                F(i) = input->func(&xvec[i], input->data);

            Eigen::Vector<double, ORDER> coeffs = Func::VLU_.solve(F);

            if (standard_error(coeffs) > input->tol)
                return false;

            coeffs_.resize(coeffs.size());
            for (int i = 0; i < coeffs.size(); ++i)
                coeffs_[i] = coeffs(i);

            leaf_ = true;
            return true;
        }
        if constexpr (D == 2) {
            Eigen::Matrix<double, ORDER, ORDER> F;
            CoeffVec xvec =
                Func::get_cheb_nodes(box_.center[0] - box_.half_length[0], box_.center[0] + box_.half_length[0]);
            CoeffVec yvec =
                Func::get_cheb_nodes(box_.center[1] - box_.half_length[1], box_.center[1] + box_.half_length[1]);

            for (int i = 0; i < ORDER; ++i) {
                for (int j = 0; j < ORDER; ++j) {
                    double x[2] = {xvec[i], yvec[j]};
                    F(i, j) = input->func(x, input->data);
                }
            }

            Eigen::Matrix<double, ORDER, ORDER> coeffs = Func::VLU_.solve(F);
            coeffs = Func::VLU_.solve(coeffs.transpose()).transpose();

            if (standard_error(coeffs) > input->tol)
                return false;

            coeffs_.resize(coeffs.size());
            for (int i = 0; i < coeffs.size(); ++i)
                coeffs_[i] = coeffs(i);

            leaf_ = true;
            return true;
        }
        if constexpr (D == 3) {
            Eigen::Tensor<double, 3> F(ORDER, ORDER, ORDER);

            CoeffVec xvec =
                Func::get_cheb_nodes(box_.center[0] - box_.half_length[0], box_.center[0] + box_.half_length[0]);
            CoeffVec yvec =
                Func::get_cheb_nodes(box_.center[1] - box_.half_length[1], box_.center[1] + box_.half_length[1]);
            CoeffVec zvec =
                Func::get_cheb_nodes(box_.center[2] - box_.half_length[2], box_.center[2] + box_.half_length[2]);

            for (int i = 0; i < ORDER; ++i) {
                for (int j = 0; j < ORDER; ++j) {
                    for (int k = 0; k < ORDER; ++k) {
                        double x[3] = {xvec[i], yvec[j], zvec[k]};
                        F(i, j, k) = input->func(x, input->data);
                    }
                }
            }

            coeffs_.resize(ORDER * ORDER * ORDER);
            Eigen::Tensor<double, 3> coeffs_tensor(ORDER, ORDER, ORDER);
            using matrix_t = Eigen::Matrix<double, ORDER, ORDER>;
            using map_t = Eigen::Map<matrix_t>;
            using tensor_t = Eigen::Tensor<double, 2>;
            for (int block = 0; block < ORDER; ++block) {
                tensor_t F_block_tensor = F.chip(block, 2);
                map_t F_block(F_block_tensor.data());

                matrix_t coeffs_tmp = Func::VLU_.solve(F_block);
                coeffs_tmp = Func::VLU_.solve(coeffs_tmp.transpose()).transpose();
                coeffs_tensor.chip(block, 2) = Eigen::TensorMap<tensor_t>(coeffs_tmp.data(), ORDER, ORDER);
            }
            for (int block = 0; block < ORDER; ++block) {
                Eigen::Tensor<double, 2> coeffs_tmp = coeffs_tensor.chip(block, 0);
                map_t coeffs_ysolve(coeffs_tmp.data());
                map_t(coeffs_.data() + block * ORDER * ORDER) = Func::VLU_.solve(coeffs_ysolve.transpose()).transpose();
            }

            for (int i = 0; i < ORDER; ++i) {
                for (int j = 0; j < ORDER; ++j) {
                    for (int k = 0; k < ORDER; ++k) {
                        VEC point =
                            (box_.center - box_.half_length).array() +
                            2.0 * VEC{(double)i, (double)j, (double)k}.array() * box_.half_length.array() / ORDER;

                        const double test_val = eval(point);
                        const double actual_val = input->func(point.data(), input->data);
                        const double rel_error = std::abs((actual_val - test_val) / actual_val);

                        if (fabs(actual_val) > 1E-16 && rel_error > input->tol) {
                            coeffs_.clear();
                            coeffs_.shrink_to_fit();
                            return false;
                        }
                    }
                }
            }

            leaf_ = true;
            return true;
        }
    }

    /// @brief eval node at point x
    /// @param[in] x point to evaluate at
    /// @returns function approximation at x
    inline double eval(const VEC &x) const { return cheb_eval<ORDER, ISET>(x, box_, coeffs_); }

    /// @brief MSGPACK serialization magic
    MSGPACK_DEFINE(box_, first_child_idx, leaf_, coeffs_);
};

/// @brief Represent a function in some domain as a tree of chebyshev nodes
/// @tparam DIM dimension of function
/// @tparam ORDER order of evaluation polynomial
/// @tparam ISET instruction set index (dummy variable to force alignment for different instruction sets)
template <int DIM, int ORDER, int ISET>
struct FunctionTree {
    static constexpr int NChild = 1 << DIM; ///< Number of children each node potentially has (2^D)
    static constexpr int Dim = DIM; ///< Dimension of tree
    static constexpr int Order = ORDER; ///< Order of tree

    using VEC = Eigen::Vector<double, DIM>; ///< D dimensional vector type
    std::vector<Node<DIM, ORDER, ISET>> nodes_; ///< Flat list of all nodes in Tree (leaf or otherwise)

    /// @brief Construct tree
    /// @param[in] input parameters for fit (function, tol, etc)
    /// @param[in] box box that this tree lives in
    FunctionTree<DIM, ORDER, ISET>(const baobzi_input_t *input, const Box<DIM, ISET> &box) {
        std::queue<Box<DIM, ISET>> q;
        VEC half_width = box.half_length * 0.5;
        q.push(box);

        uint64_t curr_child_idx = 1;
        while (!q.empty()) {
            int n_next = q.size();
            int node_index = nodes_.size();
            for (int i = 0; i < n_next; ++i) {
                Box<DIM, ISET> box = q.front();
                q.pop();

                nodes_.push_back(Node<DIM, ORDER, ISET>(box));
            }

#pragma omp parallel for
            for (size_t i = 0; i < n_next; ++i) {
                auto &node = nodes_[i + node_index];
                node.fit(input);
            }

            for (int i = 0; i < n_next; ++i) {
                auto &node = nodes_[i + node_index];
                if (!node.is_leaf()) {
                    node.first_child_idx = curr_child_idx;
                    curr_child_idx += NChild;

                    VEC &center = node.box_.center;
                    for (uint64_t child = 0; child < NChild; ++child) {
                        VEC offset;

                        // Extract sign of each offset component from the bits of child
                        // Basically: permute all possible offsets
                        for (int j = 0; j < DIM; ++j) {
                            double signed_hw[2] = {-half_width[j], half_width[j]};
                            offset[j] = signed_hw[(child >> j) & 1];
                        }

                        q.push(Box<DIM, ISET>(center + offset, half_width));
                    }
                }
            }

            half_width *= 0.5;
        }
    }

    FunctionTree<DIM, ORDER, ISET>() = default; ///< Default constructor for msgpack happiness

    /// @brief Find leaf node containing a point via standard pointer traversal
    /// @param[in] x point that the node will contain
    /// @return leaf node containing point x
    inline const Node<DIM, ORDER, ISET> &find_node_traverse(const VEC &x) const {
        auto *node = &nodes_[0];
        while (!node->is_leaf()) {
            uint64_t child_idx = 0;
            for (int i = 0; i < DIM; ++i)
                child_idx = child_idx | ((x[i] > node->box_.center[i]) << i);

            node = &nodes_[node->first_child_idx + child_idx];
        }

        return *node;
    }

    /// @brief eval function approximation at point
    /// @param[in] x point to evaluate function at
    /// @returns function approximation at point x
    inline double eval(const VEC &x) const { return find_node_traverse(x).eval(x); }

    /// @brief msgpack serialization magic
    MSGPACK_DEFINE(nodes_);
};

/// @brief Represents a function in some domain as a grid of baobzi::FunctionTree objects
/// @tparam DIM dimension of function
/// @tparam ORDER order of evaluation polynomial
/// @tparam ISET instruction set index (dummy variable to force alignment for different instruction sets)
template <int DIM, int ORDER, int ISET = 0>
class Function {
  public:
    static constexpr int NChild = 1 << DIM; ///< Number of children each node potentially has (2^D)
    static constexpr int Dim = DIM; ///< Input dimension of function
    static constexpr int Order = ORDER; ///< Order of polynomial representation
    static constexpr int ISet = ISET; ///< Instruction set (dummy param)
    static std::mutex statics_mutex; ///< mutex for locking vandermonde/chebyshev initialization

    using VEC = Eigen::Vector<double, DIM>; ///< D dimensional vector type
    using CoeffVec = Eigen::Vector<double, ORDER>; ///< Order dimensional vector type
    using VanderMat = Eigen::Matrix<double, ORDER, ORDER>; ///< VanderMonde Matrix type

    using DBox = Box<DIM, ISET>; ///< D dimensional box type

    static CoeffVec cosarray_;                  ///< Cached array of cosine values at chebyshev nodes
    static Eigen::PartialPivLU<VanderMat> VLU_; ///< Cached LU decomposition of Vandermonde matrix

    DBox box_;       ///< box representing the domain of our function
    double tol_;     ///< Desired relative tolerance of our approximation
    VEC lower_left_; ///< Bottom 'corner' of our domain

    std::vector<FunctionTree<DIM, ORDER, ISET>> subtrees_; ///< Grid of FunctionTree objects that do the work
    Eigen::Vector<int, DIM> n_subtrees_;                   ///< Number of subtrees in each linear dimension of our space
    VEC bin_size_;                                         ///< Linear dimensions of the bins that our subtrees live

    /// @brief calculate vandermonde matrix
    /// @return Vandermonde matrix for chebyshev polynomials with order=ORDER
    static VanderMat calc_vandermonde() {
        VanderMat V;

        for (int j = 0; j < ORDER; ++j) {
            V(0, j) = 1;
            V(1, j) = cosarray_(j);
        }

        for (int i = 2; i < ORDER; ++i) {
            for (int j = 0; j < ORDER; ++j) {
                V(i, j) = double(2) * V(i - 1, j) * cosarray_(j) - V(i - 2, j);
            }
        }

        return V.transpose();
    }

    /// @brief calculate chebyshev nodes on bounds [lb, ub]
    /// @param[in] lb lower bound
    /// @param[in] ub upper bound
    /// @returns vector of chebyshev nodes scaled within [lb, ub]
    static inline CoeffVec get_cheb_nodes(double lb, double ub) {
        return 0.5 * ((lb + ub) + (ub - lb) * cosarray_.array());
    }

    /// @brief initialize static class variables
    ///
    /// Modifies baobzi::Function::cosarray_, baobzi::Function::VLU_
    static void init_statics() {
        static bool is_initialized = false;
        std::lock_guard<std::mutex> lock(statics_mutex);
        if (is_initialized)
            return;

        for (int i = 0; i < ORDER; ++i)
            cosarray_[ORDER - i - 1] = cos(M_PI * (i + 0.5) / ORDER);
        VLU_ = Eigen::PartialPivLU<VanderMat>(calc_vandermonde());
        is_initialized = true;
    }

    /// @brief Construct our Function object (fits recursively, can be slow)
    /// @param[in] input parameters for fit (function, tol, etc)
    /// @param[in] xp [dim] center of function domain
    /// @param[in] lp [dim] half length of function domain
    Function<DIM, ORDER, ISET>(const baobzi_input_t *input, const double *xp, const double *lp)
        : box_(VEC(xp), VEC(lp)), tol_(input->tol) {
        init_statics();

        VEC l(lp);
        VEC x(xp);
        std::queue<DBox> q;

        for (int i = 0; i < DIM; ++i)
            n_subtrees_[i] = l[i] / l.minCoeff();

        uint8_t max_depth_ = 0;
        q.push(DBox(x, l));

        // Half-width of next children
        VEC half_width = l * 0.5;

        // Breadth first search. Step through each level of the tree and test fit all of the nodes
        // We exit when a level isn't completely filled with parent nodes (rather than leaves)
        // This way we can always avoid redundant traversals by jumping straight to a root node of a subtree
        while (!q.empty()) {
            int n_next = q.size();

            std::vector<Node<DIM, ORDER, ISET>> nodes;
            for (int i = 0; i < n_next; ++i) {
                DBox box = q.front();
                q.pop();

                nodes.emplace_back(Node<DIM, ORDER, ISET>(box));
            }

#pragma omp parallel for
            for (int i = 0; i < nodes.size(); ++i)
                nodes[i].fit(input);

            for (auto &node : nodes) {
                if (!node.is_leaf()) {
                    VEC &center = node.box_.center;
                    for (unsigned child = 0; child < NChild; ++child) {
                        VEC offset;

                        // Extract sign of each offset component from the bits of child
                        // Basically: permute all possible offsets
                        for (int j = 0; j < DIM; ++j) {
                            double signed_hw[2] = {-half_width[j], half_width[j]};
                            offset[j] = signed_hw[(child >> j) & 1];
                        }

                        q.push(DBox(center + offset, half_width));
                    }
                }
            }

            if (!q.empty())
                max_depth_++;

            half_width *= 0.5;
            if ((1 << (DIM * max_depth_)) == q.size())
                n_subtrees_ *= 2;
            else
                break;
        }

        for (int j = 0; j < DIM; ++j)
            bin_size_[j] = 2.0 * box_.half_length[j] / n_subtrees_[j];
        lower_left_ = box_.center - box_.half_length;

        subtrees_.reserve(n_subtrees_.prod());
        for (int i_bin = 0; i_bin < n_subtrees_.prod(); ++i_bin) {
            Eigen::Vector<int, DIM> bins = get_bins(i_bin);

            VEC parent_center = (bins.template cast<double>().array() + 0.5) * bin_size_.array() + lower_left_.array();

            Box<DIM, ISET> root_box = {parent_center, 0.5 * bin_size_};
            subtrees_.push_back(FunctionTree<DIM, ORDER, ISET>(input, root_box));
        }
    }

    /// @brief default constructor for msgpack magic
    Function<DIM, ORDER, ISET>() { init_statics(); };

    /// @brief convert linear bin index to [dim] bin vector
    /// @param[in] i_bin linear index
    /// @returns [dim] bin vector
    inline Eigen::Vector<int, DIM> get_bins(const int i_bin) const {
        if constexpr (DIM == 1)
            return Eigen::Vector<int, DIM>{i_bin};
        else if constexpr (DIM == 2)
            return Eigen::Vector<int, DIM>{i_bin % n_subtrees_[0], i_bin / n_subtrees_[0]};
        else if constexpr (DIM == 3)
            return Eigen::Vector<int, DIM>{i_bin % n_subtrees_[0], (i_bin / n_subtrees_[0]) % n_subtrees_[1],
                                           i_bin / (n_subtrees_[0] * n_subtrees_[1])};
    }

    /// @brief find linear index of bin at a point
    /// @param[in] x [1] position to find bin
    /// @returns linear index of bin that x lives in
    inline int get_linear_bin(const Eigen::Vector<double, 1> &x) const {
        const double x_bin = x[0] - lower_left_[0];
        return x_bin / bin_size_[0];
    }

    /// @brief find linear index of bin at a point
    /// @param[in] x [2] position to find bin
    /// @returns linear index of bin that x lives in
    inline int get_linear_bin(const Eigen::Vector2d &x) const {
        const VEC x_bin = x - lower_left_;
        const Eigen::Vector<int, DIM> bin = (x_bin.array() / bin_size_.array()).template cast<int>();
        return bin[0] + n_subtrees_[0] * bin[1];
    }

    /// @brief find linear index of bin at a point
    /// @param[in] x [3] position to find bin
    /// @returns linear index of bin that x lives in
    inline int get_linear_bin(const Eigen::Vector3d &x) const {
        const VEC x_bin = x - lower_left_;
        const Eigen::Vector<int, DIM> bin = (x_bin.array() / bin_size_.array()).template cast<int>();
        return bin[0] + n_subtrees_[0] * bin[1] + n_subtrees_[0] * n_subtrees_[1] * bin[2];
    }

    /// @brief get constant reference to leaf node that contains a point
    /// @param[in] x point of interest
    /// @returns constant reference to leaf node that contains x
    inline const Node<DIM, ORDER, ISET> &find_node(const VEC &x) const {
        return subtrees_[get_linear_bin(x)].find_node_traverse(x);
    }

    /// @brief eval function approximation at point
    /// @param[in] x point to evaluate function at
    /// @returns function approximation at point x
    inline double eval(const VEC &x) const { return find_node(x).eval(x); }

    /// @brief eval function approximation at point
    /// @param[in] xp [DIM] point to evaluate function at
    /// @returns function approximation at point xp
    inline double eval(const double *xp) const { return eval(VEC(xp)); }

    /// @brief eval function approximation at point
    /// @param[in] x [DIM] point to evaluate function at
    /// @returns function approximation at point x
    inline double operator()(const VEC &x) const { return eval(x); }

    /// @brief eval function approximation at point
    /// @param[in] x point to evaluate function at
    /// @returns function approximation at point x
    inline double operator()(const double *x) const { return eval(x); }

    /// @brief save function approximation to file
    /// @param[in] filename path to save file at
    void save(const char *filename) {
        std::ofstream ofs(filename, std::ofstream::binary | std::ofstream::out);
        baobzi_header_t params{Dim, Order, BAOBZI_HEADER_VERSION};
        msgpack::pack(ofs, params);
        msgpack::pack(ofs, *this);
    }

    /// @brief msgpack serialization magic
    MSGPACK_DEFINE_MAP(box_, subtrees_, n_subtrees_, tol_, lower_left_, bin_size_);
};

template <int DIM, int ORDER, int ISET>
std::mutex Function<DIM, ORDER, ISET>::statics_mutex;

template <int DIM, int ORDER, int ISET>
typename Function<DIM, ORDER, ISET>::CoeffVec Function<DIM, ORDER, ISET>::cosarray_;

template <int DIM, int ORDER, int ISET>
Eigen::PartialPivLU<typename Function<DIM, ORDER, ISET>::VanderMat> Function<DIM, ORDER, ISET>::VLU_;
} // namespace baobzi

#endif


#include <blitz/timer.h>

BZ_USING_NAMESPACE(blitz)

double dot(const double* a, const double* b, int n)
{
    double result = 0.;
    for (int i=0; i < n; ++i)
        result += a[i] * b[i];

    return result;
}

template<class T>
void sink(T&)
{
}

void sink(double,double,double,double,double,double,double,double,double,double)
{
}

void init(double* x, int n)
{
    // Completely arbitrary
    for (int i=0; i < n; ++i)
        x[i] = 3.4982938192839824982 * i;
}

const int nmax = 40;

int main()
{
    Timer timer;
    const int iterations1 = 5000000;

    double a1[nmax],a2[nmax],a3[nmax],a4[nmax],a5[nmax],a6[nmax],a7[nmax],a8[nmax],a9[nmax],
        a10[nmax],b1[nmax],b2[nmax],b3[nmax],b4[nmax],b5[nmax],b6[nmax],b7[nmax],b8[nmax],
        b9[nmax],b10[nmax];
    init(a1,nmax);
    init(a2,nmax);
    init(a3,nmax);
    init(a4,nmax);
    init(a5,nmax);
    init(a6,nmax);
    init(a7,nmax);
    init(a8,nmax);
    init(a9,nmax);
    init(a10,nmax);
    init(b1,nmax);
    init(b2,nmax);
    init(b3,nmax);
    init(b4,nmax);
    init(b5,nmax);
    init(b6,nmax);
    init(b7,nmax);
    init(b8,nmax);
    init(b9,nmax);
    init(b10,nmax);

    for (int n=1; n < nmax; ++n)
    {
    int iterations = iterations1 / n;

    timer.start();
    for (int i=0; i < iterations; ++i)
    {
        double result1 = dot(a1,b1,n);
        double result2 = dot(a2,b2,n);
        double result3 = dot(a3,b3,n);
        double result4 = dot(a4,b4,n);
        double result5 = dot(a5,b5,n);
        double result6 = dot(a6,b6,n);
        double result7 = dot(a7,b7,n);
        double result8 = dot(a8,b8,n);
        double result9 = dot(a9,b9,n);
        double result10 = dot(a10,b10,n);
        sink(result1,result2,result3,result4,result5,result6,result7,result8,
            result9,result10);
    }
    timer.stop();

    double Mflops = 10.0 * (n + (n-1)) * iterations / 1e+6;
    cout << n << '\t' << (Mflops/timer.elapsedSeconds()) << endl;
    }

    return 0;
}


#include <iostream>
#include "linalg.h"

using namespace std;

#include <ctime>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

#define MATRIX_SIZE 10

int main(int argc, char **argv) {
    Matrix<float, MATRIX_SIZE, MATRIX_SIZE> A
      = MatrixXf::Random(MATRIX_SIZE, MATRIX_SIZE);
    A = A * A.transpose();
    Matrix<float, MATRIX_SIZE, 1> b = MatrixXf::Random(MATRIX_SIZE, 1);

    MatrixXf x = LinearEquationSolve(A, b);
    cout << "Result is " << x.transpose() << endl;
    return 0;
}


//  (C) Copyright Jeremy Murphy 2015.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/config.hpp>
#define BOOST_TEST_MAIN
#include <boost/array.hpp>
#include <boost/math/tools/polynomial.hpp>
#include <boost/math/common_factor_rt.hpp>
#include <boost/mpl/list.hpp>
#include <boost/mpl/joint_view.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <utility>

using namespace boost::math::tools;
using namespace std;

template <typename T>
struct answer
{
    answer(std::pair< polynomial<T>, polynomial<T> > const &x) :
    quotient(x.first), remainder(x.second) {}

    polynomial<T> quotient;
    polynomial<T> remainder;
};

boost::array<double, 4> const d3a = {{10, -6, -4, 3}};
boost::array<double, 4> const d3b = {{-7, 5, 6, 1}};
boost::array<double, 4> const d3c = {{10.0/3.0, -2.0, -4.0/3.0, 1.0}};
boost::array<double, 2> const d1a = {{-2, 1}};
boost::array<double, 3> const d2a = {{-2, 2, 3}};
boost::array<double, 3> const d2b = {{-7, 5, 6}};
boost::array<double, 3> const d2c = {{31, -21, -22}};
boost::array<double, 1> const d0a = {{6}};
boost::array<double, 1> const d0b = {{3}};

boost::array<int, 9> const d8 = {{-5, 2, 8, -3, -3, 0, 1, 0, 1}};
boost::array<int, 9> const d8b = {{0, 2, 8, -3, -3, 0, 1, 0, 1}};
boost::array<int, 7> const d6 = {{21, -9, -4, 0, 5, 0, 3}};
boost::array<int, 3> const d2 = {{-6, 0, 9}};
boost::array<int, 6> const d5 = {{-9, 0, 3, 0, -15}};

BOOST_AUTO_TEST_CASE( test_construction )
{
    polynomial<double> const a(d3a.begin(), d3a.end());
    polynomial<double> const b(d3a.begin(), 3);
    BOOST_CHECK_EQUAL(a, b);
}

#ifndef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
BOOST_AUTO_TEST_CASE( test_initializer_list_construction )
{
    polynomial<double> a(begin(d3a), end(d3a));
    polynomial<double> b = {10, -6, -4, 3};
    polynomial<double> c{{10, -6, -4, 3}};
    BOOST_CHECK_EQUAL(a, b);
    BOOST_CHECK_EQUAL(b, c);
}
#endif

BOOST_AUTO_TEST_CASE( test_degree )
{
    polynomial<double> const zero = zero_element(std::multiplies< polynomial<double> >());
    polynomial<double> const a(d3a.begin(), d3a.end());
    BOOST_CHECK_THROW(zero.degree(), std::logic_error);
    BOOST_CHECK_EQUAL(a.degree(), 3u);
}

BOOST_AUTO_TEST_CASE( test_division_over_field )
{
    polynomial<double> const a(d3a.begin(), d3a.end());
    polynomial<double> const b(d1a.begin(), d1a.end());
    polynomial<double> const q(d2a.begin(), d2a.end());
    polynomial<double> const r(d0a.begin(), d0a.end());
    polynomial<double> const c(d3b.begin(), d3b.end());
    polynomial<double> const d(d2b.begin(), d2b.end());
    polynomial<double> const e(d2c.begin(), d2c.end());
    polynomial<double> const f(d0b.begin(), d0b.end());
    polynomial<double> const g(d3c.begin(), d3c.end());
    polynomial<double> const zero = zero_element(std::multiplies< polynomial<double> >());
    polynomial<double> const one = identity_element(std::multiplies< polynomial<double> >());

    answer<double> result = quotient_remainder(a, b);
    BOOST_CHECK_EQUAL(result.quotient, q);
    BOOST_CHECK_EQUAL(result.remainder, r);
    BOOST_CHECK_EQUAL(a, q * b + r); // Sanity check.

    result = quotient_remainder(a, c);
    BOOST_CHECK_EQUAL(result.quotient, f);
    BOOST_CHECK_EQUAL(result.remainder, e);
    BOOST_CHECK_EQUAL(a, f * c + e); // Sanity check.

    result = quotient_remainder(a, f);
    BOOST_CHECK_EQUAL(result.quotient, g);
    BOOST_CHECK_EQUAL(result.remainder, zero);
    BOOST_CHECK_EQUAL(a, g * f + zero); // Sanity check.
    // Check that division by a regular number gives the same result.
    BOOST_CHECK_EQUAL(a / 3.0, g);
    BOOST_CHECK_EQUAL(a % 3.0, zero);

    // Sanity checks.
    BOOST_CHECK_EQUAL(a / a, one);
    BOOST_CHECK_EQUAL(a % a, zero);
    // BOOST_CHECK_EQUAL(zero / zero, zero); // TODO
}

BOOST_AUTO_TEST_CASE( test_division_over_ufd )
{
    polynomial<int> const zero = zero_element(std::multiplies< polynomial<int> >());
    polynomial<int> const one = identity_element(std::multiplies< polynomial<int> >());
    polynomial<int> const aa(d8.begin(), d8.end());
    polynomial<int> const bb(d6.begin(), d6.end());
    polynomial<int> const q(d2.begin(), d2.end());
    polynomial<int> const r(d5.begin(), d5.end());

    answer<int> result = quotient_remainder(aa, bb);
    BOOST_CHECK_EQUAL(result.quotient, q);
    BOOST_CHECK_EQUAL(result.remainder, r);

    // Sanity checks.
    BOOST_CHECK_EQUAL(aa / aa, one);
    BOOST_CHECK_EQUAL(aa % aa, zero);
}

BOOST_AUTO_TEST_CASE( test_gcd )
{
    /* NOTE: Euclidean gcd is not yet customized to return THE greatest
     * common polynomial divisor. If d is THE greatest common divisior of u and
     * v, then gcd(u, v) will return d or -d according to the algorithm.
     * By convention, it should return d, as for example Maxima and Wolfram
     * Alpha do.
     * This test is an example of the fact that it returns -d.
     */
    boost::array<double, 9> const d8 = {{105, 278, -88, -56, 16}};
    boost::array<double, 7> const d6 = {{70, 232, -44, -64, 16}};
    boost::array<double, 7> const d2 = {{-35, 24, -4}};
    polynomial<double> const u(d8.begin(), d8.end());
    polynomial<double> const v(d6.begin(), d6.end());
    polynomial<double> const w(d2.begin(), d2.end());
    polynomial<double> const d = boost::math::gcd(u, v);
    BOOST_CHECK_EQUAL(w, d);
}

// Sanity checks to make sure I didn't break it.
typedef boost::mpl::list<int, long, boost::multiprecision::cpp_int> integral_test_types;
typedef boost::mpl::list<double, boost::multiprecision::cpp_rational, boost::multiprecision::cpp_bin_float_single, boost::multiprecision::cpp_dec_float_50> non_integral_test_types;
typedef boost::mpl::joint_view<integral_test_types, non_integral_test_types> all_test_types;

BOOST_AUTO_TEST_CASE_TEMPLATE( test_addition, T, all_test_types )
{
    polynomial<T> const a(d3a.begin(), d3a.end());
    polynomial<T> const b(d1a.begin(), d1a.end());
    polynomial<T> const zero = zero_element(multiplies< polynomial<T> >());

    polynomial<T> result = a + b; // different degree
    boost::array<T, 4> tmp = {{8, -5, -4, 3}};
    polynomial<T> expected(tmp.begin(), tmp.end());
    BOOST_CHECK_EQUAL(result, expected);
    BOOST_CHECK_EQUAL(a + zero, a);
    BOOST_CHECK_EQUAL(a + b, b + a);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_subtraction, T, all_test_types )
{
    polynomial<T> const a(d3a.begin(), d3a.end());
    polynomial<T> const zero = zero_element(multiplies< polynomial<T> >());

    BOOST_CHECK_EQUAL(a - T(0), a);
    BOOST_CHECK_EQUAL(T(0) - a, -a);
    BOOST_CHECK_EQUAL(a - zero, a);
    BOOST_CHECK_EQUAL(zero - a, -a);
    BOOST_CHECK_EQUAL(a - a, zero);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_multiplication, T, all_test_types )
{
    polynomial<T> const a(d3a.begin(), d3a.end());
    polynomial<T> const b(d1a.begin(), d1a.end());
    polynomial<T> const zero = zero_element(multiplies< polynomial<T> >());

    BOOST_CHECK_EQUAL(a * T(0), zero);
    BOOST_CHECK_EQUAL(a * zero, zero);
    BOOST_CHECK_EQUAL(zero * T(0), zero);
    BOOST_CHECK_EQUAL(zero * zero, zero);
    BOOST_CHECK_EQUAL(a * b, b * a);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_arithmetic_relations, T, all_test_types )
{
    polynomial<T> const a(d8b.begin(), d8b.end());
    polynomial<T> const b(d1a.begin(), d1a.end());

    BOOST_CHECK_EQUAL(a * T(2), a + a);
    BOOST_CHECK_EQUAL(a - b, -b + a);
    BOOST_CHECK_EQUAL(a, (a * a) / a);
    BOOST_CHECK_EQUAL(a, (a / a) * a);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_non_integral_arithmetic_relations, T, non_integral_test_types )
{
    polynomial<T> const a(d8b.begin(), d8b.end());
    polynomial<T> const b(d1a.begin(), d1a.end());

    BOOST_CHECK_EQUAL(a * T(0.5), a / T(2));
}


// Copyright (C) 2008-2018 Lorenzo Caminiti
// Distributed under the Boost Software License, Version 1.0 (see accompanying
// file LICENSE_1_0.txt or a copy at http://www.boost.org/LICENSE_1_0.txt).
// See: http://www.boost.org/doc/libs/release/libs/contract/doc/html/index.html

//[n1962_sqrt
#include <boost/contract.hpp>
#include <cmath>
#include <cassert>

long lsqrt(long x) {
    long result;
    boost::contract::check c = boost::contract::function()
        .precondition([&] {
            BOOST_CONTRACT_ASSERT(x >= 0);
        })
        .postcondition([&] {
            BOOST_CONTRACT_ASSERT(result * result <= x);
            BOOST_CONTRACT_ASSERT((result + 1) * (result + 1) > x);
        })
    ;

    return result = long(std::sqrt(double(x)));
}

int main() {
    assert(lsqrt(4) == 2);
    return 0;
}
//]


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/**
 * Test_PolyEval.cpp - Homomorphic Polynomial Evaluation
 */
#include <NTL/ZZ.h>
NTL_CLIENT
#include "polyEval.h"
#include "EncryptedArray.h"

#ifdef DEBUG_PRINTOUT
extern FHESecKey* dbgKey;
extern EncryptedArray* dbgEa;
#endif

static bool noPrint = false;

bool testEncrypted(long d, const EncryptedArray& ea,
		   const FHESecKey& secretKey)
{
  const FHEcontext& context = ea.getContext();
  const FHEPubKey& publicKey = secretKey;
  long p = publicKey.getPtxtSpace();
  zz_pBak bak; bak.save(); zz_p::init(p);
  zz_pXModulus phimX = conv<zz_pX>(context.zMStar.getPhimX());

  // Choose random plaintext polynomials
  zz_pX pX = random_zz_pX(deg(phimX)-1);
  Vec<zz_pX> ppoly(INIT_SIZE, d);
  for (long i=0; i<ppoly.length(); i++) random(ppoly[i], deg(phimX)-1);

  // Evaluate the non-encrypted polynomial
  zz_pX pres = (ppoly.length()>0)? ppoly[ppoly.length()-1] : zz_pX::zero();
  for (long i=ppoly.length()-2; i>=0; i--) {
    MulMod(pres, pres, pX, phimX);
    pres += ppoly[i];
  }

  // Encrypt the random polynomials
  Ctxt cX(publicKey);
  Vec<Ctxt> cpoly(INIT_SIZE, d, cX);
  secretKey.Encrypt(cX, conv<ZZX>(pX));
  for (long i=0; i<ppoly.length(); i++)
    secretKey.Encrypt(cpoly[i], conv<ZZX>(ppoly[i]));

  // Evaluate the encrypted polynomial
  polyEval(cX, cpoly, cX);

  // Compare the results
  ZZX ret;
  secretKey.Decrypt(ret, cX);
  zz_pX cres = conv<zz_pX>(ret);
  bool success = (cres == pres);
  if (success) std::cout << " encrypted poly match, ";
  else         std::cout << " encrypted poly MISMATCH\n";
  return success;
}

void testIt(long d, long k, long p, long r, long m, long L,
	    bool isMonic=false)
{
  FHEcontext context(m, p, r);
  long p2r = context.alMod.getPPowR();
  buildModChain(context, L, /*c=*/3);
  EncryptedArray ea(context);

  FHESecKey secretKey(context);
  const FHEPubKey& publicKey = secretKey;
  secretKey.GenSecKey(/*w=*/64);// A Hamming-weight-64 secret key
  //  addSome1DMatrices(secretKey); // compute key-switching matrices

#ifdef DEBUG_PRINTOUT
  dbgEa = &ea;        // for debugging purposes
  dbgKey = &secretKey;
#endif

  if (!noPrint) std::cout << (isDryRun()? "* dry run, " : "* ")
		     << "degree-"<<d<<", m="<<m<<", L="<<L<<", p^r="<<p2r<<endl;

  // evaluate encrypted poly at encrypted point
  if (!testEncrypted(d, ea, secretKey)) exit(0);

  // evaluate at random points (at least one co-prime with p)
  vector<long> x;
  ea.random(x);
  while (GCD(x[0],p)!=1) { x[0] = RandomBnd(p2r); }
  Ctxt inCtxt(publicKey), outCtxt(publicKey);
  ea.encrypt(inCtxt, publicKey, x);

  ZZX poly;
  for (long i=d; i>=0; i--)
    SetCoeff(poly, i, RandomBnd(p2r)); // coefficients are random
  if (isMonic) SetCoeff(poly, d);    // set top coefficient to 1

  // Evaluate poly on the ciphertext
  polyEval(outCtxt, poly, inCtxt, k);

  // Check the result
  vector<long> y;
  ea.decrypt(outCtxt, secretKey, y);
  for (long i=0; i<ea.size(); i++) {
    long ret = polyEvalMod(poly, x[i], p2r);
    if (ret != y[i]) {
      std::cout << "plaintext poly MISMATCH\n";
      exit(0);
    }
  }
  std::cout << "plaintext poly match\n" << std::flush;
}

void usage(char *prog)
{
  std::cout << "Usage: "<<prog<<" [ optional parameters ]...\n";
  std::cout << "  optional parameters have the form 'attr1=val1 attr2=val2 ...'\n";
  std::cout << "  dry=1 for dry run [default=0]\n";
  std::cout << "  p is the plaintext base [default=3]" << endl;
  std::cout << "  r is the lifting [default=2]" << endl;
  std::cout << "  m is a specific cyclotomic ring\n";
  std::cout << "  d is the polynomial degree [default=undefined]" << endl;
  std::cout << "    d=undefined means trying a few powers d=1,...,4,25,...,34"<<endl;
  std::cout << "  k is the baby-step parameter [default=undefined]" << endl;
  std::cout << "    if k is undefined it is computed from d" << endl;
  std::cout << "  noPrint suppresses printouts [default=0]" << endl;
  exit(0);
}

int main(int argc, char *argv[])
{
  argmap_t argmap;
  argmap["p"] = "3";
  argmap["r"] = "2";
  argmap["m"] = "0";
  argmap["d"] = "-1";
  argmap["k"] = "0";
  argmap["dry"] = "0";
  argmap["noPrint"] = "0";

  // get parameters from the command line
  if (!parseArgs(argc, argv, argmap)) usage(argv[0]);

  long p = atoi(argmap["p"]);
  long r = atoi(argmap["r"]);
  long m = atoi(argmap["m"]);
  long d = atoi(argmap["d"]);
  long k = atoi(argmap["k"]);
  bool dry = atoi(argmap["dry"]);
  noPrint = atoi(argmap["noPrint"]);

  long max_d = (d<=0)? 35 : d;
  long L = 5+NextPowerOfTwo(max_d);
  if (m<2)
    m = FindM(/*secprm=*/80, L, /*c=*/3, p, 1, 0, m, !noPrint);
  setDryRun(dry);

  // Test both monic and non-monic polynomials of this degree
  if (d>=0) {
    testIt(d, k, p, r, m, L, false);
    testIt(d, k, p, r, m, L, true);
    return 0;
  }

  // Test degrees 1 to 4 and 25 through 35
  testIt(1, k, p, r, m, L, true);
  testIt(3, k, p, r, m, L, true);
  for (d=25; d<=33; d+=2)
    testIt(d, k, p, r, m, L);

  return 0;
}


#include "project/ExponentialSmoothing.hpp"
#include "project/vecPush.hpp"
#include <armadillo>
#include <math.h>

ExponentialSmoothing::ExponentialSmoothing(arma::vec &time_series, float alpha){
    param_alpha = alpha;
    ts = time_series;
}

arma::vec calculateWeigths(float alpha, int length) {
    arma::vec weigths = arma::zeros(length);
    weigths(0) = alpha;
    for (int ii = 1; ii < length; ii++){
        weigths(ii) = alpha*pow((1-alpha),ii);
    };

    return weigths;
}

void ExponentialSmoothing::fit(){
    weigths = calculateWeigths(param_alpha, ts.n_rows);
}

float weigthedMean(arma::vec &x, arma::vec &weigths){
    float weigthed_mean = arma::sum(x%weigths)/arma::sum(weigths);
    return weigthed_mean;
}

void vec_push(arma::vec &v, float value) {
    arma::vec av(1);
    av.at(0) = value;
    v.insert_rows(v.n_rows, av.row(0));
}

arma::vec ExponentialSmoothing::forecast(int horizon){
    arma::vec forecast = arma::zeros(horizon);
    arma::vec tmp_ts = ts;
    for (int ii = 0; ii < horizon; ii++){
        float point_forecast = weigthedMean(tmp_ts, weigths);
        forecast(ii) = point_forecast;
        vec_push(tmp_ts, )
    }
}


#include "problemes.h"
#include "arithmetique.h"

#include <fstream>
#include <boost/algorithm/string.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;
typedef std::vector<vecteur> matrice;

ENREGISTRER_PROBLEME(99, "Largest exponential") {
    // Comparing two numbers written in index form like 211 and 37 is not difficult, as any calculator would confirm
    // that 2^11 = 2048 < 3^7 = 2187.
    //
    // However, confirming that 632382^518061 > 519432^525806 would be much more difficult, as both numbers contain over
    // three million digits.
    //
    // Using base_exp.txt (right click and 'Save Link/Target As...'), a 22K text file containing one thousand lines with
    // a base/exponent pair on each line, determine which line number has the greatest numerical value.
    //
    // NOTE: The first two lines in the file represent the numbers in the example given above.
    std::ifstream ifs("data/p099_base_exp.txt");
    nombre resultat = 0;
    nombre numero_ligne = 1;
    std::string ligne;
    long double maximum = 0;
    while (ifs >> ligne) {
        std::vector<std::string> v;
        boost::split(v, ligne, boost::is_any_of(","));
        const long double log = std::log(std::stold(v.front())) * std::stold(v.back());
        if (log > maximum) {
            resultat = numero_ligne;
            maximum = log;
        }
        ++numero_ligne;
    }
    return std::to_string(resultat);
}


#ifndef IP3_DET_MODEL_HPP_INCLUDED
#define IP3_DET_MODEL_HPP_INCLUDED
#include <vector>
#include <iostream>
#include <cmath>
#include <boost/math/constants/constants.hpp>
#include "astron_utility_functions.hpp"
//const double pi = boost::math::constants::pi<double>();

//DECLATATIONS FOR IP3_SYSTEM
class IP3
{
   private:

      double a2 = 0.2E-03;     //    0.2         1/(micro M * sec)
      double d1 = 0.13;   //     0.13        micro M
      double d2 = 1.049;  //     1.049       micro M
      double d3 = 0.9434;   //   0.9434      micro M
      double d5 = 0.08234; //    0.08234     micro M

      double c0 = 2.0;  //       2.0         micro M
      double c1 = 0.185;   //                Dimensionless
      double v1 = 6.0E-03; //        6.0         1/sec
      double v2 = 0.11E-03;//        0.11        1/sec
      double v3 = 0.9E-03;     //    0.9         1/ (micro M * sec)
      double k3 = 0.1;   //      0.1         micro M

   public:

      double CaER = 0.0;      // micro M (Calcium concentration in the ER)
      double ip3_conc = 0.0;      // micro M (Given IP3 concentration in the cytosol)
      double ca_conc = 0.0;

      double ip3_tau = 0.0;
      double ip3_rate = 0.0;
      double ip3_thres = 0.0;
      double ip3_gen = 0.0;
      double ip3_equ = 0.0;

//------------------ CONSTRUCTORS

   /* IP3 class implicit constructor */
   IP3(): CaER(0.0), ca_conc(0.0), ip3_conc(0.0)
   {
   };

  /* ASTRO class explicit constructor */
   IP3( double CaER_, double ca_conc_, double ip3_conc_ ): CaER(CaER_), ca_conc(ca_conc_), ip3_conc(ip3_conc_)
   {
   };
//------------------ Function declarations
   void set_CaER();

   double m_inf();
   double h_inf();
   double h_tau();

   template <class State, class Deriv >
   void operator() ( const State &x, Deriv &dxdt , const double  t );
};
//--------------------m_inf Function
double IP3::m_inf()
{
   double value =  ( ip3_conc / ( ip3_conc + d1 ) ) * ( ca_conc / (ca_conc + d5) ) ;
   return value;
};
//--------------------h_tau Function
double IP3::h_tau()
{
   double Q2 = d2 * ((ip3_conc + d1)/(ip3_conc + d3));
   double value  = 1 / (a2 * (Q2 + ca_conc) );
   return value;
}

//--------------------h_inf Function
double IP3::h_inf()
{
   double Q2 = d2 * ((ip3_conc + d1)/(ip3_conc + d3));
   double value = Q2/(Q2+ca_conc);
   return value;
};
//--------------------Set CaER Function
void IP3::set_CaER()
{
   this->CaER = (c0 - ca_conc) / c1;
   //std::cout << "Set_CaER = " << (c0 - ca_conc)/c1 << "\t" << CaER << "\n";
};
//------- ASTRO class ODE Function
template <class State, class Deriv >
void IP3::operator() ( const State &x_, Deriv &dxdt_ , const double t )
{
   typename boost::range_iterator< const State >::type x = boost::begin( x_ );
   typename boost::range_iterator< Deriv >::type dxdt = boost::begin( dxdt_ );

   //std::cout << CaER << "\n";

   ca_conc = x[1]; // Set Calcium concentration
   ip3_conc = x[2]; // Set ip3 concentration

   set_CaER(); // Update ER calcium level

   dxdt[0] = ( h_inf() - x[0] ) / h_tau() ; // dh/dt

   double JCh = ( c1 * v1 * pow(m_inf(),3) * pow(x[0],3) * (x[1] - CaER) ); //   J_channel
   double JPump = ( v3 * pow(x[1],2) ) / ( pow(k3,2) + pow(x[1],2) ) ;   //    J_Pump
   double JLeak = c1 * v2 * ( x[1] - CaER );   //    J_Leak

   dxdt[1] = - (JCh + JPump + JLeak) ;// dCa/dt
   dxdt[2] =  (1/ip3_tau) * (ip3_equ - x[2])  + (ip3_rate * heaviside(ip3_gen,ip3_thres)) ;//dip3/dt

};

#endif // IP3_DET_MODEL_HPP_INCLUDED


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

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

 * See LICENSE for the license information

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

/**
 * testTriangulation.cpp
 *
 *  Created on: July 30th, 2013
 *      Author: cbeall3
 */

#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <CppUnitLite/TestHarness.h>

#include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp>

using namespace std;
using namespace gtsam;
using namespace boost::assign;

// Some common constants

static const boost::shared_ptr<Cal3_S2> sharedCal = //
    boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);

// Looking along X-axis, 1 meter above ground plane (x-y)
static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);

// create second camera 1 meter to the right of first camera
static const Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
PinholeCamera<Cal3_S2> camera2(pose2, *sharedCal);

// landmark ~5 meters infront of camera
static const Point3 landmark(5, 0.5, 1.2);

// 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark);

//******************************************************************************
// Simple test with a well-behaved two camera situation
TEST( triangulation, twoPoses) {

  vector<Pose3> poses;
  Point2Vector measurements;

  poses += pose1, pose2;
  measurements += z1, z2;

  double rank_tol = 1e-9;

  // 1. Test simple DLT, perfect in no noise situation
  bool optimize = false;
  boost::optional<Point3> actual1 = //
      triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
  EXPECT(assert_equal(landmark, *actual1, 1e-7));

  // 2. test with optimization on, same answer
  optimize = true;
  boost::optional<Point3> actual2 = //
      triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
  EXPECT(assert_equal(landmark, *actual2, 1e-7));

  // 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
  measurements.at(0) += Point2(0.1, 0.5);
  measurements.at(1) += Point2(-0.2, 0.3);
  optimize = false;
  boost::optional<Point3> actual3 = //
      triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4));

  // 4. Now with optimization on
  optimize = true;
  boost::optional<Point3> actual4 = //
      triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
}

//******************************************************************************
// Similar, but now with Bundler calibration
TEST( triangulation, twoPosesBundler) {

  boost::shared_ptr<Cal3Bundler> bundlerCal = //
      boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480);
  PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal);
  PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal);

  // 1. Project two landmarks into two cameras and triangulate
  Point2 z1 = camera1.project(landmark);
  Point2 z2 = camera2.project(landmark);

  vector<Pose3> poses;
  Point2Vector measurements;

  poses += pose1, pose2;
  measurements += z1, z2;

  bool optimize = true;
  double rank_tol = 1e-9;

  boost::optional<Point3> actual = //
      triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize);
  EXPECT(assert_equal(landmark, *actual, 1e-7));

  // Add some noise and try again
  measurements.at(0) += Point2(0.1, 0.5);
  measurements.at(1) += Point2(-0.2, 0.3);

  boost::optional<Point3> actual2 = //
      triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize);
  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4));
}

//******************************************************************************
TEST( triangulation, fourPoses) {
  vector<Pose3> poses;
  Point2Vector measurements;

  poses += pose1, pose2;
  measurements += z1, z2;

  boost::optional<Point3> actual = triangulatePoint3(poses, sharedCal,
      measurements);
  EXPECT(assert_equal(landmark, *actual, 1e-2));

  // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
  measurements.at(0) += Point2(0.1, 0.5);
  measurements.at(1) += Point2(-0.2, 0.3);

  boost::optional<Point3> actual2 = //
      triangulatePoint3(poses, sharedCal, measurements);
  EXPECT(assert_equal(landmark, *actual2, 1e-2));

  // 3. Add a slightly rotated third camera above, again with measurement noise
  Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
  PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
  Point2 z3 = camera3.project(landmark);

  poses += pose3;
  measurements += z3 + Point2(0.1, -0.1);

  boost::optional<Point3> triangulated_3cameras = //
      triangulatePoint3(poses, sharedCal, measurements);
  EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));

  // Again with nonlinear optimization
  boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses,
      sharedCal, measurements, 1e-9, true);
  EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));

  // 4. Test failure: Add a 4th camera facing the wrong way
  Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
  PinholeCamera<Cal3_S2> camera4(pose4, *sharedCal);

#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
  CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);

  poses += pose4;
  measurements += Point2(400, 400);

  CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
      TriangulationCheiralityException);
#endif
}

//******************************************************************************
TEST( triangulation, fourPoses_distinct_Ks) {
  Cal3_S2 K1(1500, 1200, 0, 640, 480);
  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  PinholeCamera<Cal3_S2> camera1(pose1, K1);

  // create second camera 1 meter to the right of first camera
  Cal3_S2 K2(1600, 1300, 0, 650, 440);
  PinholeCamera<Cal3_S2> camera2(pose2, K2);

  // 1. Project two landmarks into two cameras and triangulate
  Point2 z1 = camera1.project(landmark);
  Point2 z2 = camera2.project(landmark);

  CameraSet<PinholeCamera<Cal3_S2> > cameras;
  Point2Vector measurements;

  cameras += camera1, camera2;
  measurements += z1, z2;

  boost::optional<Point3> actual = //
      triangulatePoint3(cameras, measurements);
  EXPECT(assert_equal(landmark, *actual, 1e-2));

  // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
  measurements.at(0) += Point2(0.1, 0.5);
  measurements.at(1) += Point2(-0.2, 0.3);

  boost::optional<Point3> actual2 = //
      triangulatePoint3(cameras, measurements);
  EXPECT(assert_equal(landmark, *actual2, 1e-2));

  // 3. Add a slightly rotated third camera above, again with measurement noise
  Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
  Cal3_S2 K3(700, 500, 0, 640, 480);
  PinholeCamera<Cal3_S2> camera3(pose3, K3);
  Point2 z3 = camera3.project(landmark);

  cameras += camera3;
  measurements += z3 + Point2(0.1, -0.1);

  boost::optional<Point3> triangulated_3cameras = //
      triangulatePoint3(cameras, measurements);
  EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));

  // Again with nonlinear optimization
  boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(cameras,
      measurements, 1e-9, true);
  EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));

  // 4. Test failure: Add a 4th camera facing the wrong way
  Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
  Cal3_S2 K4(700, 500, 0, 640, 480);
  PinholeCamera<Cal3_S2> camera4(pose4, K4);

#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
  CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);

  cameras += camera4;
  measurements += Point2(400, 400);
  CHECK_EXCEPTION(triangulatePoint3(cameras, measurements),
      TriangulationCheiralityException);
#endif
}

//******************************************************************************
TEST( triangulation, outliersAndFarLandmarks) {
  Cal3_S2 K1(1500, 1200, 0, 640, 480);
  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  PinholeCamera<Cal3_S2> camera1(pose1, K1);

  // create second camera 1 meter to the right of first camera
  Cal3_S2 K2(1600, 1300, 0, 650, 440);
  PinholeCamera<Cal3_S2> camera2(pose2, K2);

  // 1. Project two landmarks into two cameras and triangulate
  Point2 z1 = camera1.project(landmark);
  Point2 z2 = camera2.project(landmark);

  CameraSet<PinholeCamera<Cal3_S2> > cameras;
  Point2Vector measurements;

  cameras += camera1, camera2;
  measurements += z1, z2;

  double landmarkDistanceThreshold = 10; // landmark is closer than that
  TriangulationParameters params(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold
  TriangulationResult actual = triangulateSafe(cameras,measurements,params);
  EXPECT(assert_equal(landmark, *actual, 1e-2));
  EXPECT(actual.valid());

  landmarkDistanceThreshold = 4; // landmark is farther than that
  TriangulationParameters params2(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold
  actual = triangulateSafe(cameras,measurements,params2);
  EXPECT(actual.farPoint());

  // 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER)
  Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
  Cal3_S2 K3(700, 500, 0, 640, 480);
  PinholeCamera<Cal3_S2> camera3(pose3, K3);
  Point2 z3 = camera3.project(landmark);

  cameras += camera3;
  measurements += z3 + Point2(10, -10);

  landmarkDistanceThreshold = 10; // landmark is closer than that
  double outlierThreshold = 100; // loose, the outlier is going to pass
  TriangulationParameters params3(1.0, false, landmarkDistanceThreshold,outlierThreshold);
  actual = triangulateSafe(cameras,measurements,params3);
  EXPECT(actual.valid());

  // now set stricter threshold for outlier rejection
  outlierThreshold = 5; // tighter, the outlier is not going to pass
  TriangulationParameters params4(1.0, false, landmarkDistanceThreshold,outlierThreshold);
  actual = triangulateSafe(cameras,measurements,params4);
  EXPECT(actual.outlier());
}

//******************************************************************************
TEST( triangulation, twoIdenticalPoses) {
  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);

  // 1. Project two landmarks into two cameras and triangulate
  Point2 z1 = camera1.project(landmark);

  vector<Pose3> poses;
  Point2Vector measurements;

  poses += pose1, pose1;
  measurements += z1, z1;

  CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
      TriangulationUnderconstrainedException);
}

//******************************************************************************
TEST( triangulation, onePose) {
  // we expect this test to fail with a TriangulationUnderconstrainedException
  // because there's only one camera observation

  vector<Pose3> poses;
  Point2Vector measurements;

  poses += Pose3();
  measurements += Point2(0,0);

  CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
      TriangulationUnderconstrainedException);
}

//******************************************************************************
TEST( triangulation, StereotriangulateNonlinear ) {

  auto stereoK = boost::make_shared<Cal3_S2Stereo>(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612);

  // two camera poses m1, m2
  Matrix4 m1, m2;
  m1 << 0.796888717,     0.603404026,   -0.0295271487, 46.6673779,
      0.592783835,    -0.77156583,    0.230856632,   66.2186159,
      0.116517574,   -0.201470143,     -0.9725393, -4.28382528,
      0, 0, 0, 1;

  m2 << -0.955959025,    -0.29288915,   -0.0189328569, 45.7169799,
      -0.29277519,    0.947083213,    0.131587097, 65.843136,
     -0.0206094928,   0.131334858,   -0.991123524, -4.3525033,
     0, 0, 0, 1;

  typedef CameraSet<StereoCamera> Cameras;
  Cameras cameras;
  cameras.push_back(StereoCamera(Pose3(m1), stereoK));
  cameras.push_back(StereoCamera(Pose3(m2), stereoK));

  StereoPoint2Vector measurements;
  measurements += StereoPoint2(226.936, 175.212, 424.469);
  measurements += StereoPoint2(339.571, 285.547, 669.973);

  Point3 initial = Point3(46.0536958, 66.4621179, -6.56285929);  // error: 96.5715555191

  Point3 actual = triangulateNonlinear(cameras, measurements, initial);

  Point3 expected(46.0484569, 66.4710686, -6.55046613); // error: 0.763510644187

  EXPECT(assert_equal(expected, actual, 1e-4));

  // regular stereo factor comparison - expect very similar result as above
  {
    typedef GenericStereoFactor<Pose3,Point3> StereoFactor;

    Values values;
    values.insert(Symbol('x', 1), Pose3(m1));
    values.insert(Symbol('x', 2), Pose3(m2));
    values.insert(Symbol('l', 1), initial);

    NonlinearFactorGraph graph;
    static SharedNoiseModel unit(noiseModel::Unit::Create(3));
    graph.emplace_shared<StereoFactor>(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK);
    graph.emplace_shared<StereoFactor>(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK);

    const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9);
    graph.addPrior(Symbol('x',1), Pose3(m1), posePrior);
    graph.addPrior(Symbol('x',2), Pose3(m2), posePrior);

    LevenbergMarquardtOptimizer optimizer(graph, values);
    Values result = optimizer.optimize();

    EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
  }

  // use Triangulation Factor directly - expect same result as above
  {
    Values values;
    values.insert(Symbol('l', 1), initial);

    NonlinearFactorGraph graph;
    static SharedNoiseModel unit(noiseModel::Unit::Create(3));

    graph.emplace_shared<TriangulationFactor<StereoCamera> >(cameras[0], measurements[0], unit, Symbol('l',1));
    graph.emplace_shared<TriangulationFactor<StereoCamera> >(cameras[1], measurements[1], unit, Symbol('l',1));

    LevenbergMarquardtOptimizer optimizer(graph, values);
    Values result = optimizer.optimize();

    EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
  }

  // use ExpressionFactor - expect same result as above
  {
    Values values;
    values.insert(Symbol('l', 1), initial);

    NonlinearFactorGraph graph;
    static SharedNoiseModel unit(noiseModel::Unit::Create(3));

    Expression<Point3> point_(Symbol('l',1));
    Expression<StereoCamera> camera0_(cameras[0]);
    Expression<StereoCamera> camera1_(cameras[1]);
    Expression<StereoPoint2> project0_(camera0_, &StereoCamera::project2, point_);
    Expression<StereoPoint2> project1_(camera1_, &StereoCamera::project2, point_);

    graph.addExpressionFactor(unit, measurements[0], project0_);
    graph.addExpressionFactor(unit, measurements[1], project1_);

    LevenbergMarquardtOptimizer optimizer(graph, values);
    Values result = optimizer.optimize();

    EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
  }
}

//******************************************************************************
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
//******************************************************************************


#include <vector>
#include <iostream>
#include <iomanip>
#include <string>
#include <fstream>

#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/lexical_cast.hpp>

#include "SpmvBase.h"
#include "MaxSLiCInterface.h"

#include <dfesnippets/sparse/common.hpp>
#include <dfesnippets/sparse/sparse_matrix.hpp>
#include <dfesnippets/sparse/partition.hpp>

#include "fpga.hpp"
// #define DEBUG_PRINT_MATRICES
// #define DEBUG_PARTITIONS

using namespace std;

string check_file(char **argv) {
  string path{argv[2]};
  ifstream f{path};
  if (!f) {
    cout << "Error opening input file" << endl;
    exit(1);
  }
  return path;
}

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

  cout << "Program arguments:" << endl;
  for (int i = 0; i < argc; i++)
    cout << "   " << argv[i] << endl;

  string path = check_file(argv);
  int num_repeat = boost::lexical_cast<int>(argv[3]);

  // -- Design Parameters
  int numPipes = SpmvBase_numPipes;

  // -- Matrix Parameters
  int n, nnzs;
  double* values;
  int *col_ind, *row_ptr;

  read_ge_mm_csr((char *)path.c_str(), &n, &nnzs, &col_ind, &row_ptr, &values);

  // adjust from 1 indexed CSR (used by MKL) to 0 indexed CSR
  for (int i = 0; i < nnzs; i++)
    col_ind[i]--;

  using namespace boost::numeric;
  // generate multiplicand
  vector<double> v(n);
  ublas::vector<double> vu(n);
  for (int i = 1; i <=n; i++) {
    v[i - 1] = i;
    vu(i - 1) = i;
  }

  // -- load the CSR matrix
  CsrMatrix<> inMatrix(n, n);

  for (int i = 0; i < n; ++i)
  {
          int rowStart = row_ptr[i] - 1;
          int rowEnd = row_ptr[i + 1] - 1;
          for (int j = rowStart; j < rowEnd; ++j)
          {
                  inMatrix(i, col_ind[j]) = values[j];
          }
  }

  auto res = ublas::prod(inMatrix, vu);
  vector<double> bExp = SpMV_MKL_ge((char *)path.c_str(), v);

  int partitionSize = min(SpmvBase_vectorCacheSize, n);
  vector<CsrMatrix<double>> partitions = partition(inMatrix, partitionSize);
  vector<double> dfe_res(n, 0);

  AdjustedCsrMatrix<double> full_original_matrix(n);
  full_original_matrix.load_from_csr(
      &inMatrix.value_data()[0],
      &inMatrix.index2_data()[0],
      &inMatrix.index1_data()[0]);

#ifdef DEBUG_PRINT_MATRICES
  cout << "Full original matrix " << endl;
  full_original_matrix.print_dense();
  cout << "Vector: " << endl;
  print_vector(v);
#endif

  int offset = 0;
  for (size_t i = 0; i < partitions.size(); ++i) {
    auto p = partitions[i];
    AdjustedCsrMatrix<double> original_matrix(n);
    original_matrix.load_from_csr(
        &p.value_data()[0],
        &p.index2_data()[0],
        &p.index1_data()[0]
        );

#ifdef DEBUG_PRINT_MATRICES
    original_matrix.print();
    original_matrix.print_dense();
#endif

    // find expected result
    vector<double> vblock(v.begin() + offset, v.begin() + offset + partitionSize);
    auto b = SpMV_DFE(original_matrix, vblock, numPipes, num_repeat);
    for (int j = 0; j < n; ++j)
    {
      dfe_res[j] += b[j];
    }
    offset += partitionSize;
  }

  cout << "Checking ublas " << endl;
  for (int i = 0; i < n; ++i) {
    if (!dfesnippets::numeric_utils::almost_equal(res(i), bExp[i])) {
      cerr << "Expected " << bExp[i] << " got: " << res(i) << endl;
      exit(1);
    }
  }
  cout << "Ran SPMV " << endl;

  int errors = 0;
  for (size_t i = 0; i < dfe_res.size(); i++)
    if (!dfesnippets::numeric_utils::almost_equal(bExp[i], dfe_res[i])) {
      cerr << "Expected [ " << i << " ] " << bExp[i] << " got: " << dfe_res[i] << endl;
      errors++;
    }

  if (errors != 0) {
    cerr << "Errors " << errors <<endl;
    return 1;
  }

  cout << "Test passed!" << endl;
  return 0;
}