#ifndef VEXCL_MBA_HPP
#define VEXCL_MBA_HPP

/*
The MIT License

Copyright (c) 2012-2018 Denis Demidov <dennis.demidov@gmail.com>

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

/**
 * \file   vexcl/mba.hpp
 * \author Denis Demidov <dennis.demidov@gmail.com>
 * \brief  Scattered data interpolation with multilevel B-Splines.
 */

#include <vector>
#include <array>
#include <sstream>
#include <memory>
#include <algorithm>
#include <numeric>
#include <type_traits>
#include <cassert>

#include <boost/tuple/tuple.hpp>
#include <boost/fusion/adapted/boost_tuple.hpp>

#include <vexcl/operations.hpp>

// Include boost.preprocessor header if variadic templates are not available.
// Also include it if we use gcc v4.6.
// This is required due to bug http://gcc.gnu.org/bugzilla/show_bug.cgi?id=35722
#if defined(BOOST_NO_VARIADIC_TEMPLATES) || (defined(__GNUC__) && !defined(__clang__) && __GNUC__ == 4 && __GNUC_MINOR__ == 6)
#  include <boost/preprocessor/repetition.hpp>
#  ifndef VEXCL_MAX_ARITY
#    define VEXCL_MAX_ARITY BOOST_PROTO_MAX_ARITY
#  endif
#endif
namespace vex {

struct mba_terminal {};

typedef vector_expression<
    typename boost::proto::terminal< mba_terminal >::type
    > mba_terminal_expression;

template <class MBA, class ExprTuple>
struct mba_interp : public mba_terminal_expression {
    typedef typename MBA::value_type value_type;

    const MBA      &cloud;
    const ExprTuple coord;

    mba_interp(const MBA &cloud, const ExprTuple coord)
        : cloud(cloud), coord(coord) {}
};

namespace detail {
    // Compile time value of N^M.
    template <size_t N, size_t M>
    struct power : std::integral_constant<size_t, N * power<N, M-1>::value> {};

    template <size_t N>
    struct power<N, 0> : std::integral_constant<size_t, 1> {};

    // Nested loop counter of compile-time size (M loops of size N).
    template <size_t N, size_t M>
    class scounter {
        public:
            scounter() : idx(0) {
                std::fill(i.begin(), i.end(), static_cast<size_t>(0));
            }

            size_t operator[](size_t d) const {
                return i[d];
            }

            scounter& operator++() {
                for(size_t d = M; d--; ) {
                    if (++i[d] < N) break;
                    i[d] = 0;
                }

                ++idx;

                return *this;
            }

            operator size_t() const {
                return idx;
            }

            bool valid() const {
                return idx < power<N, M>::value;
            }
        private:
            size_t idx;
            std::array<size_t, M> i;
    };

    // Nested loop counter of run-time size (M loops of given sizes).
    template <size_t M>
    class dcounter {
        public:
            dcounter(const std::array<size_t, M> &N)
                : idx(0),
                  size(std::accumulate(N.begin(), N.end(),
                            static_cast<size_t>(1), std::multiplies<size_t>())),
                  N(N)
            {
                std::fill(i.begin(), i.end(), static_cast<size_t>(0));
            }

            size_t operator[](size_t d) const {
                return i[d];
            }

            dcounter& operator++() {
                for(size_t d = M; d--; ) {
                    if (++i[d] < N[d]) break;
                    i[d] = 0;
                }

                ++idx;

                return *this;
            }

            operator size_t() const {
                return idx;
            }

            bool valid() const {
                return idx < size;
            }
        private:
            size_t idx, size;
            std::array<size_t, M> N, i;
    };
} // namespace detail

/// Scattered data interpolation with multilevel B-Splines.
template <size_t NDIM, typename real = double>
class mba {
    public:
        typedef real value_type;
        typedef std::array<real,   NDIM> point;
        typedef std::array<size_t, NDIM> index;

        static const size_t ndim = NDIM;

        std::vector< backend::command_queue >       queue;
        std::vector< backend::device_vector<real> > phi;
        point xmin, hinv;
        index n, stride;

        /** Creates the approximation functor.
         * `cmin` and `cmax` specify the domain boundaries, `coo` and `val`
         * contain coordinates and values of the data points. `grid` is the
         * initial control grid size. The approximation hierarchy will have at
         * most `levels` and will stop when the desired approximation precision
         * `tol` will be reached.
         */
        mba(
                const std::vector<backend::command_queue> &queue,
                const point &cmin, const point &cmax,
                const std::vector<point> &coo, std::vector<real> val,
                std::array<size_t, NDIM> grid, size_t levels = 8, real tol = 1e-8
           ) : queue(queue)
        {
            init(cmin, cmax, coo.begin(), coo.end(), val.begin(), grid, levels, tol);
        }

        /** Creates the approximation functor.
         * `cmin` and `cmax` specify the domain boundaries. Coordinates and
         * values of the data points are passed as iterator ranges. `grid` is
         * the initial control grid size. The approximation hierarchy will have
         * at most `levels` and will stop when the desired approximation
         * precision `tol` will be reached.
         */
        template <class CooIter, class ValIter>
        mba(
                const std::vector<backend::command_queue> &queue,
                const point &cmin, const point &cmax,
                CooIter coo_begin, CooIter coo_end, ValIter val_begin,
                std::array<size_t, NDIM> grid, size_t levels = 8, real tol = 1e-8
           ) : queue(queue)
        {
            init(cmin, cmax, coo_begin, coo_end, val_begin, grid, levels, tol);
        }

#if !defined(BOOST_NO_VARIADIC_TEMPLATES) && ((!defined(__GNUC__) || (__GNUC__ > 4 || __GNUC__ == 4 && __GNUC_MINOR__ > 6)) || defined(__clang__))
        /// Provide interpolated values at given coordinates.
        template <class... Expr>
        auto operator()(const Expr&... expr) const ->
            mba_interp< mba, boost::tuple<const Expr&...> >
        {
            static_assert(sizeof...(Expr) == NDIM, "Wrong number of parameters");
            return mba_interp< mba, boost::tuple<const Expr&...> >(*this, boost::tie(expr...));
        }
#else

#define VEXCL_FUNCALL_OPERATOR(z, n, data)                                     \
  template <BOOST_PP_ENUM_PARAMS(n, class Expr)>                               \
  mba_interp<mba, boost::tuple<BOOST_PP_ENUM_BINARY_PARAMS(                    \
                      n, const Expr, &BOOST_PP_INTERCEPT)> >                   \
  operator()(BOOST_PP_ENUM_BINARY_PARAMS(n, const Expr, &expr)) {              \
    return mba_interp<mba, boost::tuple<BOOST_PP_ENUM_BINARY_PARAMS(           \
                               n, const Expr, &BOOST_PP_INTERCEPT)> >(         \
        *this, boost::tie(BOOST_PP_ENUM_PARAMS(n, expr)));                     \
  }

BOOST_PP_REPEAT_FROM_TO(1, 10, VEXCL_FUNCALL_OPERATOR, ~)

#undef VEXCL_FUNCALL_OPERATOR
#endif
    private:
        template <class CooIter, class ValIter>
        void init(
                const point &cmin, const point &cmax,
                CooIter coo_begin, CooIter coo_end, ValIter val_begin,
                std::array<size_t, NDIM> grid, size_t levels = 8, real tol = 1e-8
                )
        {
            for(size_t k = 0; k < NDIM; ++k)
                assert(grid[k] > 1);

            double res0 = std::accumulate(
                    val_begin, val_begin + (coo_end - coo_begin),
                    static_cast<real>(0),
                    [](real sum, real v) { return sum + v * v; }
                    );

            std::unique_ptr<lattice> psi(
                    new lattice(cmin, cmax, grid, coo_begin, coo_end, val_begin)
                    );
            double res = psi->update_data(coo_begin, coo_end, val_begin);
#ifdef VEXCL_MBA_VERBOSE
            std::cout << "level  0: res = " << std::scientific << res << std::endl;
#endif

            for (size_t k = 1; (res > res0 * tol) && (k < levels); ++k) {
                for(size_t d = 0; d < NDIM; ++d) grid[d] = 2 * grid[d] - 1;

                std::unique_ptr<lattice> f(
                        new lattice(cmin, cmax, grid, coo_begin, coo_end, val_begin)
                        );
                res = f->update_data(coo_begin, coo_end, val_begin);
#ifdef VEXCL_MBA_VERBOSE
                std::cout << "level " << k << std::scientific << ": res = " << res << std::endl;
#endif

                f->append_refined(*psi);
                psi = std::move(f);
            }

            xmin   = psi->xmin;
            hinv   = psi->hinv;
            n      = psi->n;
            stride = psi->stride;

            phi.reserve(queue.size());

            for(auto q = queue.begin(); q != queue.end(); ++q)
                phi.push_back( backend::device_vector<real>(
                            *q, psi->phi.size(), psi->phi.data(), backend::MEM_READ_ONLY
                            ) );
        }

        // Control lattice.
        struct lattice {
            point xmin, hinv;
            index n, stride;
            std::vector<real> phi;

            template <class CooIter, class ValIter>
            lattice(
                    const point &cmin, const point &cmax, std::array<size_t, NDIM> grid,
                    CooIter coo_begin, CooIter coo_end, ValIter val_begin
                   ) : xmin(cmin), n(grid)
            {
                for(size_t d = 0; d < NDIM; ++d) {
                    hinv[d] = (grid[d] - 1) / (cmax[d] - cmin[d]);
                    xmin[d] -= 1 / hinv[d];
                    n[d]    += 2;
                }

                stride[NDIM - 1] = 1;
                for(size_t d = NDIM - 1; d--; )
                    stride[d] = stride[d + 1] * n[d + 1];

                std::vector<real> delta(n[0] * stride[0], 0.0);
                std::vector<real> omega(n[0] * stride[0], 0.0);

                auto p = coo_begin;
                auto v = val_begin;
                for(; p != coo_end; ++p, ++v) {
                    if (!contained(cmin, cmax, *p)) continue;

                    index i;
                    point s;

                    for(size_t d = 0; d < NDIM; ++d) {
                        real u = ((*p)[d] - xmin[d]) * hinv[d];
                        i[d] = static_cast<size_t>(std::floor(u) - 1);
                        s[d] = u - std::floor(u);
                    }

                    std::array<real, detail::power<4, NDIM>::value> w;
                    real sw2 = 0;

                    for(detail::scounter<4, NDIM> d; d.valid(); ++d) {
                        real buf = 1;
                        for(size_t k = 0; k < NDIM; ++k)
                            buf *= B(d[k], s[k]);

                        w[d] = buf;
                        sw2 += buf * buf;
                    }

                    for(detail::scounter<4, NDIM> d; d.valid(); ++d) {
                        real phi = (*v) * w[d] / sw2;

                        size_t idx = 0;
                        for(size_t k = 0; k < NDIM; ++k) {
                            assert(i[k] + d[k] < n[k]);

                            idx += (i[k] + d[k]) * stride[k];
                        }

                        real w2 = w[d] * w[d];

                        assert(idx < delta.size());

                        delta[idx] += w2 * phi;
                        omega[idx] += w2;
                    }
                }

                phi.resize(omega.size());

                for(auto w = omega.begin(), d = delta.begin(), f = phi.begin();
                        w != omega.end();
                        ++w, ++d, ++f
                   )
                {
                    if (std::fabs(*w) < 1e-32)
                        *f = 0;
                    else
                        *f = (*d) / (*w);
                }
            }

            // Get interpolated value at given position.
            real operator()(const point &p) const {
                index i;
                point s;

                for(size_t d = 0; d < NDIM; ++d) {
                    real u = (p[d] - xmin[d]) * hinv[d];
                    i[d] = static_cast<size_t>(std::floor(u) - 1);
                    s[d] = u - std::floor(u);
                }

                real f = 0;

                for(detail::scounter<4, NDIM> d; d.valid(); ++d) {
                    real w = 1;
                    for(size_t k = 0; k < NDIM; ++k)
                        w *= B(d[k], s[k]);

                    f += w * get(i, d);
                }

                return f;
            }

            // Subtract interpolated values from data points.
            template <class CooIter, class ValIter>
            real update_data(
                    CooIter coo_begin, CooIter coo_end, ValIter val_begin
                    ) const
            {
                auto c = coo_begin;
                auto v = val_begin;

                real res = 0;

                for(; c != coo_end; ++c, ++v) {
                    *v -= (*this)(*c);

                    res += (*v) * (*v);
                }

                return res;
            }

            // Refine r and append it to the current control lattice.
            void append_refined(const lattice &r) {
                static const std::array<real, 5> s = {{
                    0.125, 0.500, 0.750, 0.500, 0.125
                }};

                for(detail::dcounter<NDIM> i(r.n); i.valid(); ++i) {
                    real f = r.phi[i];
                    for(detail::scounter<5, NDIM> d; d.valid(); ++d) {
                        index j;
                        bool skip = false;
                        size_t idx = 0;
                        for(size_t k = 0; k < NDIM; ++k) {
                            j[k] = 2 * i[k] + d[k] - 3;
                            if (j[k] >= n[k]) { skip = true; break; }

                            idx += j[k] * stride[k];
                        }

                        if (skip) continue;

                        real c = 1;
                        for(size_t k = 0; k < NDIM; ++k) c *= s[d[k]];

                        phi[idx] += f * c;
                    }
                }
            }

            private:
                // Value of k-th B-Spline at t.
                static inline real B(size_t k, real t) {
                    assert(0 <= t && t < 1);
                    assert(k < 4);

                    switch (k) {
                        case 0:
                            return (t * (t * (-t + 3) - 3) + 1) / 6;
                        case 1:
                            return (t * t * (3 * t - 6) + 4) / 6;
                        case 2:
                            return (t * (t * (-3 * t + 3) + 3) + 1) / 6;
                        case 3:
                        default:
                            return t * t * t / 6;
                    }
                }

                // x is within [xmin, xmax].
                static bool contained(
                        const point &xmin, const point &xmax, const point &x)
                {
                    for(size_t d = 0; d < NDIM; ++d) {
                        static const real eps = 1e-12;

                        if (x[d] - eps <  xmin[d]) return false;
                        if (x[d] + eps >= xmax[d]) return false;
                    }

                    return true;
                }

                // Get value of phi at index (i + d).
                template <class Shift>
                inline real get(const index &i, const Shift &d) const {
                    size_t idx = 0;

                    for(size_t k = 0; k < NDIM; ++k) {
                        size_t j = i[k] + d[k];

                        if (j >= n[k]) return 0;
                        idx += j * stride[k];
                    }

                    return phi[idx];
                }
        };
};

namespace traits {

template <>
struct is_vector_expr_terminal< mba_terminal > : std::true_type {};

template <>
struct proto_terminal_is_value< mba_terminal > : std::true_type {};

template <class MBA, class ExprTuple>
struct terminal_preamble< mba_interp<MBA, ExprTuple> > {
    static void get(backend::source_generator &src,
            const mba_interp<MBA, ExprTuple>&,
            const backend::command_queue&, const std::string &prm_name,
            detail::kernel_generator_state_ptr)
    {
        typedef typename MBA::value_type real;

        std::string B = prm_name + "_B";

        src.begin_function<real>(B + "0");
        src.begin_function_parameters();
        src.template parameter<real>("t");
        src.end_function_parameters();
        src.new_line() << "return (t * (t * (-t + 3) - 3) + 1) / 6;";
        src.end_function();

        src.begin_function<real>(B + "1");
        src.begin_function_parameters();
        src.template parameter<real>("t");
        src.end_function_parameters();
        src.new_line() << "return (t * t * (3 * t - 6) + 4) / 6;";
        src.end_function();

        src.begin_function<real>(B + "2");
        src.begin_function_parameters();
        src.template parameter<real>("t");
        src.end_function_parameters();
        src.new_line() << "return (t * (t * (-3 * t + 3) + 3) + 1) / 6;";
        src.end_function();

        src.begin_function<real>(B + "3");
        src.begin_function_parameters();
        src.template parameter<real>("t");
        src.end_function_parameters();
        src.new_line() << "return t * t * t / 6;";
        src.end_function();

        src.begin_function<real>(prm_name + "_mba");
        src.begin_function_parameters();

        for(size_t k = 0; k < MBA::ndim; ++k)
            src.template parameter<real>("x") << k;

        for(size_t k = 0; k < MBA::ndim; ++k) {
            src.template parameter<real>("c" + std::to_string(k));
            src.template parameter<real>("h" + std::to_string(k));
            src.parameter<size_t>("n" + std::to_string(k));
            src.parameter<size_t>("m" + std::to_string(k));
        }

        src.template parameter< global_ptr<const real> >("phi");
        src.end_function_parameters();
        src.new_line() << type_name<real>() << " u;";
        for(size_t k = 0; k < MBA::ndim; ++k) {
            src.new_line() << "u = (x" << k << " - c" << k << ") * h" << k << ";";
            src.new_line() << type_name<size_t>() << " i" << k << " = floor(u) - 1;";
            src.new_line() << type_name<real>() << " s" << k << " = u - floor(u);";
        }
        src.new_line() << type_name<real>() << " f = 0;";
        src.new_line() << type_name<size_t>() << " j, idx;";

        for(detail::scounter<4,MBA::ndim> d; d.valid(); ++d) {
            src.new_line() << "idx = 0;";
            for(size_t k = 0; k < MBA::ndim; ++k) {
                src.new_line() << "j = i" << k << " + " << d[k] << ";";
                src.new_line() << "if (j < n" << k << ")";
                src.open("{").new_line() << "idx += j * m" << k << ";";
            }

            src.new_line() << "f += ";
            for(size_t k = 0; k < MBA::ndim; ++k) {
                if (k) src << " * ";
                src << B << d[k] << "(s" << k << ")";
            }

            src << " * phi[idx];";

            for(size_t k = 0; k < MBA::ndim; ++k)
                src.close("}");
        }
        src.new_line() << "return f;";
        src.end_function();
    }
};

template <class MBA, class ExprTuple>
struct kernel_param_declaration< mba_interp<MBA, ExprTuple> > {
    static void get(backend::source_generator &src,
            const mba_interp<MBA, ExprTuple> &term,
            const backend::command_queue &queue, const std::string &prm_name,
            detail::kernel_generator_state_ptr state)
    {
        typedef typename MBA::value_type real;
        boost::fusion::for_each(term.coord, prmdecl(src, queue, prm_name, state));

        for(size_t k = 0; k < MBA::ndim; ++k) {
            src.template parameter<real>(prm_name + "_c" + std::to_string(k));
            src.template parameter<real>(prm_name + "_h" + std::to_string(k));
            src.parameter<size_t>(prm_name + "_n" + std::to_string(k));
            src.parameter<size_t>(prm_name + "_m" + std::to_string(k));
        }

        src.parameter< global_ptr<const real> >(prm_name + "_phi");
    }

    struct prmdecl {
        backend::source_generator &s;
        const backend::command_queue &queue;
        const std::string &prm_name;
        detail::kernel_generator_state_ptr state;
        mutable int pos;

        prmdecl(backend::source_generator &s,
                const backend::command_queue &queue, const std::string &prm_name,
                detail::kernel_generator_state_ptr state
            ) : s(s), queue(queue), prm_name(prm_name), state(state), pos(0)
        {}

        template <class Expr>
        void operator()(const Expr &expr) const {
            std::ostringstream prefix;
            prefix << prm_name << "_x" << pos;
            detail::declare_expression_parameter ctx(s, queue, prefix.str(), state);
            detail::extract_terminals()(boost::proto::as_child(expr), ctx);

            pos++;
        }
    };
};

template <class MBA, class ExprTuple>
struct local_terminal_init< mba_interp<MBA, ExprTuple> > {
    static void get(backend::source_generator &src,
            const mba_interp<MBA, ExprTuple> &term,
            const backend::command_queue &queue, const std::string &prm_name,
            detail::kernel_generator_state_ptr state)
    {
        boost::fusion::for_each(term.coord, local_init(src, queue, prm_name, state));
    }

    struct local_init {
        backend::source_generator &s;
        const backend::command_queue &queue;
        const std::string &prm_name;
        detail::kernel_generator_state_ptr state;
        mutable int pos;

        local_init(backend::source_generator &s,
                const backend::command_queue &queue, const std::string &prm_name,
                detail::kernel_generator_state_ptr state
            ) : s(s), queue(queue), prm_name(prm_name), state(state), pos(0)
        {}

        template <class Expr>
        void operator()(const Expr &expr) const {
            std::ostringstream prefix;
            prefix << prm_name << "_x" << pos;

            detail::output_local_preamble init_ctx(s, queue, prefix.str(), state);
            boost::proto::eval(boost::proto::as_child(expr), init_ctx);

            pos++;
        }
    };
};

template <class MBA, class ExprTuple>
struct partial_vector_expr< mba_interp<MBA, ExprTuple> > {
    static void get(backend::source_generator &src,
            const mba_interp<MBA, ExprTuple> &term,
            const backend::command_queue &queue, const std::string &prm_name,
            detail::kernel_generator_state_ptr state)
    {
        src << prm_name << "_mba(";

        boost::fusion::for_each(term.coord, buildexpr(src, queue, prm_name, state));

        for(size_t k = 0; k < MBA::ndim; ++k) {
            src << ", " << prm_name << "_c" << k
                << ", " << prm_name << "_h" << k
                << ", " << prm_name << "_n" << k
                << ", " << prm_name << "_m" << k;
        }

        src << ", " << prm_name << "_phi)";
    }

    struct buildexpr {
        backend::source_generator &s;
        const backend::command_queue &queue;
        const std::string &prm_name;
        detail::kernel_generator_state_ptr state;
        mutable int pos;

        buildexpr(backend::source_generator &s,
                const backend::command_queue &queue, const std::string &prm_name,
                detail::kernel_generator_state_ptr state
            ) : s(s), queue(queue), prm_name(prm_name), state(state), pos(0)
        {}

        template <class Expr>
        void operator()(const Expr &expr) const {
            if(pos) s << ", ";

            std::ostringstream prefix;
            prefix << prm_name << "_x" << pos;

            detail::vector_expr_context ctx(s, queue, prefix.str(), state);
            boost::proto::eval(boost::proto::as_child(expr), ctx);

            pos++;
        }
    };
};

template <class MBA, class ExprTuple>
struct kernel_arg_setter< mba_interp<MBA, ExprTuple> > {
    static void set(const mba_interp<MBA, ExprTuple> &term,
            backend::kernel &kernel, unsigned part, size_t index_offset,
            detail::kernel_generator_state_ptr state)
    {

        boost::fusion::for_each(term.coord,
                setargs(kernel, part, index_offset, state));

        for(size_t k = 0; k < MBA::ndim; ++k) {
            kernel.push_arg(term.cloud.xmin[k]);
            kernel.push_arg(term.cloud.hinv[k]);
            kernel.push_arg(term.cloud.n[k]);
            kernel.push_arg(term.cloud.stride[k]);
        }
        kernel.push_arg(term.cloud.phi[part]);
    }

    struct setargs {
        backend::kernel &kernel;
        unsigned part;
        size_t index_offset;
        detail::kernel_generator_state_ptr state;

        setargs(
                backend::kernel &kernel, unsigned part, size_t index_offset,
                detail::kernel_generator_state_ptr state
               )
            : kernel(kernel), part(part), index_offset(index_offset), state(state)
        {}

        template <class Expr>
        void operator()(const Expr &expr) const {
            detail::set_expression_argument ctx(kernel, part, index_offset, state);
            detail::extract_terminals()( boost::proto::as_child(expr), ctx);
        }
    };
};

template <class MBA, class ExprTuple>
struct expression_properties< mba_interp<MBA, ExprTuple> > {
    static void get(const mba_interp<MBA, ExprTuple> &term,
            std::vector<backend::command_queue> &queue_list,
            std::vector<size_t> &partition,
            size_t &size
            )
    {
        boost::fusion::for_each(term.coord, extrprop(queue_list, partition, size));
    }

    struct extrprop {
        std::vector<backend::command_queue> &queue_list;
        std::vector<size_t> &partition;
        size_t &size;

        extrprop(std::vector<backend::command_queue> &queue_list,
            std::vector<size_t> &partition, size_t &size
            ) : queue_list(queue_list), partition(partition), size(size)
        {}

        template <class Expr>
        void operator()(const Expr &expr) const {
            if (queue_list.empty()) {
                detail::get_expression_properties prop;
                detail::extract_terminals()(boost::proto::as_child(expr), prop);

                queue_list = prop.queue;
                partition  = prop.part;
                size       = prop.size;
            }
        }
    };
};

} //namespace traits

} // namespace vex

#endif


// Copyright 2019 Fred Hutchinson Cancer Research Center
// See the included LICENSE file for details on the licence that is granted to the user of this software.
#include <cytolib/compensation.hpp>
#include <boost/foreach.hpp>

namespace cytolib
{
	compensation::compensation(mat spillMat, vector<string> _markers){
		if(spillMat.n_cols!=spillMat.n_rows)
			throw(domain_error("non-squared shaped spillover matrix!"));
		if(spillMat.n_cols!=_markers.size())
			throw(domain_error("The number of columns of the spillover matrix is not the same as the markers!"));

		marker = _markers;
		mat B = spillMat.t();

		spillOver.resize(spillMat.n_elem);
		memcpy(spillOver.data(), B.mem, sizeof(double)*spillMat.n_elem);
	}
	/**
	 * convert spillover matrix from row-majored std::vector
	 * to col-majored arma::ma
	 * @return
	 */
	mat compensation::get_spillover_mat ()const
	{

		unsigned nMarker = marker.size();
		if(spillOver.size()!=nMarker * nMarker)
			throw(domain_error("invalid spillover matrix!"));
		mat B(spillOver.data(), nMarker, nMarker);
		return B.t();
	}
	void compensation::update_channels(const CHANNEL_MAP & chnl_map){

		for(vector<string>::iterator it = marker.begin(); it != marker.end(); it++)
		{
			string curName = *it;

			CHANNEL_MAP::const_iterator itChnl = chnl_map.find(curName);
			if(itChnl!=chnl_map.end())
				*it = itChnl->second;
		}
	}
	void compensation::convertToPb(pb::COMP & comp_pb){
		comp_pb.set_cid(cid);
		comp_pb.set_name(name);
		comp_pb.set_prefix(prefix);
		comp_pb.set_suffix(suffix);
		comp_pb.set_comment(comment);
		BOOST_FOREACH(vector<double>::value_type & it, spillOver){
			comp_pb.add_spillover(it);
		}
		BOOST_FOREACH(vector<string>::value_type & it, marker){
				comp_pb.add_marker(it);
			}
	}
	compensation::compensation(const pb::COMP & comp_pb):cid(comp_pb.cid()),prefix(comp_pb.prefix()),suffix(comp_pb.suffix()),name(comp_pb.name()),comment(comp_pb.comment()){
		for(int i = 0; i < comp_pb.marker_size(); i++)
			marker.push_back(comp_pb.marker(i));
		for(int i = 0; i < comp_pb.spillover_size(); i++)
				spillOver.push_back(comp_pb.spillover(i));
	}

};


#include <shared_mutex>
#include <dlib/dnn.h>
#include <dlib/image_loader/image_loader.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/graph_utils.h>
#include "facerec.h"
#include "jpeg_mem_loader.h"
#include "classify.h"

using namespace dlib;

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual = add_prev1<block<N,BN,1,tag1<SUBNET>>>;

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual_down = add_prev2<avg_pool<2,2,2,2,skip1<tag2<block<N,BN,2,tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block  = BN<con<N,3,3,1,1,relu<BN<con<N,3,3,stride,stride,SUBNET>>>>>;

template <int N, typename SUBNET> using ares      = relu<residual<block,N,affine,SUBNET>>;
template <int N, typename SUBNET> using ares_down = relu<residual_down<block,N,affine,SUBNET>>;

template <typename SUBNET> using alevel0 = ares_down<256,SUBNET>;
template <typename SUBNET> using alevel1 = ares<256,ares<256,ares_down<256,SUBNET>>>;
template <typename SUBNET> using alevel2 = ares<128,ares<128,ares_down<128,SUBNET>>>;
template <typename SUBNET> using alevel3 = ares<64,ares<64,ares<64,ares_down<64,SUBNET>>>>;
template <typename SUBNET> using alevel4 = ares<32,ares<32,ares<32,SUBNET>>>;

template <long num_filters, typename SUBNET> using con5d = con<num_filters,5,5,2,2,SUBNET>;
template <long num_filters, typename SUBNET> using con5  = con<num_filters,5,5,1,1,SUBNET>;

template <typename SUBNET> using downsampler  = relu<affine<con5d<32, relu<affine<con5d<32, relu<affine<con5d<16,SUBNET>>>>>>>>>;
template <typename SUBNET> using rcon5  = relu<affine<con5<45,SUBNET>>>;

using cnn_anet_type = loss_mmod<con<1,9,9,1,1,rcon5<rcon5<rcon5<downsampler<input_rgb_image_pyramid<pyramid_down<6>>>>>>>>;

using anet_type = loss_metric<fc_no_bias<128,avg_pool_everything<
                            alevel0<
                            alevel1<
                            alevel2<
                            alevel3<
                            alevel4<
                            max_pool<3,3,2,2,relu<affine<con<32,7,7,2,2,
                            input_rgb_image_sized<150>
                            >>>>>>>>>>>>;

static const size_t RECT_LEN = 4;
static const size_t DESCR_LEN = 128;
static const size_t SHAPE_LEN = 2;
static const size_t RECT_SIZE = RECT_LEN * sizeof(long);
static const size_t DESCR_SIZE = DESCR_LEN * sizeof(float);
static const size_t SHAPE_SIZE = SHAPE_LEN * sizeof(long);

static std::vector<matrix<rgb_pixel>> jitter_image(
    const matrix<rgb_pixel>& img,
    int count
);

class FaceRec {
public:
	FaceRec(const char* model_dir) {
		detector_ = get_frontal_face_detector();

		std::string dir = model_dir;
		std::string shape_predictor_path = dir + "/shape_predictor_5_face_landmarks.dat";
		std::string resnet_path = dir + "/dlib_face_recognition_resnet_model_v1.dat";
		std::string cnn_resnet_path = dir + "/mmod_human_face_detector.dat";

		deserialize(shape_predictor_path) >> sp_;
		deserialize(resnet_path) >> net_;
		deserialize(cnn_resnet_path) >> cnn_net_;

		jittering = 0;
		size = 150;
		padding = 0.25;
	}

	std::tuple<std::vector<rectangle>, std::vector<descriptor>, std::vector<full_object_detection>>
	Recognize(const matrix<rgb_pixel>& img,int max_faces,int type) {
		std::vector<rectangle> rects;
		std::vector<descriptor> descrs;
		std::vector<full_object_detection> shapes;

		if(type == 0) {
			std::lock_guard<std::mutex> lock(detector_mutex_);
			rects = detector_(img);
		} else{
			std::lock_guard<std::mutex> lock(cnn_net_mutex_);
			auto dets = cnn_net_(img);
            for (auto&& d : dets) {
                rects.push_back(d.rect);
            }
		}

		// Short circuit.
		if (rects.size() == 0 || (max_faces > 0 && rects.size() > (size_t)max_faces))
			return {std::move(rects), std::move(descrs), std::move(shapes)};

		std::sort(rects.begin(), rects.end());

		for (const auto& rect : rects) {
			auto shape = sp_(img, rect);
			shapes.push_back(shape);
			matrix<rgb_pixel> face_chip;
			extract_image_chip(img, get_face_chip_details(shape, size, padding), face_chip);
			std::lock_guard<std::mutex> lock(net_mutex_);
			if (jittering > 0) {
				descrs.push_back(mean(mat(net_(jitter_image(std::move(face_chip), jittering)))));
			} else {
				descrs.push_back(net_(face_chip));
			}
		}

		return {std::move(rects), std::move(descrs), std::move(shapes)};
	}

	void SetSamples(std::vector<descriptor>&& samples, std::vector<int>&& cats) {
		std::unique_lock<std::shared_mutex> lock(samples_mutex_);
		samples_ = std::move(samples);
		cats_ = std::move(cats);
	}

	int Classify(const descriptor& test_sample, float tolerance) {
		std::shared_lock<std::shared_mutex> lock(samples_mutex_);
		return classify(samples_, cats_, test_sample, tolerance);
	}

    void Config(unsigned long new_size, double new_padding, int new_jittering) {
        size = new_size;
        padding = new_padding;
        jittering = new_jittering;
    }
private:
	std::mutex detector_mutex_;
	std::mutex net_mutex_;
	std::mutex cnn_net_mutex_;
	std::shared_mutex samples_mutex_;
	frontal_face_detector detector_;
	shape_predictor sp_;
	anet_type net_;
	cnn_anet_type cnn_net_;
	std::vector<descriptor> samples_;
	std::vector<int> cats_;
	int jittering;
	unsigned long size;
	double padding;
};

// Plain C interface for Go.

facerec* facerec_init(const char* model_dir) {
	facerec* rec = (facerec*)calloc(1, sizeof(facerec));
	try {
		FaceRec* cls = new FaceRec(model_dir);
		rec->cls = (void*)cls;
	} catch(serialization_error& e) {
		rec->err_str = strdup(e.what());
		rec->err_code = SERIALIZATION_ERROR;
	} catch (std::exception& e) {
		rec->err_str = strdup(e.what());
		rec->err_code = UNKNOWN_ERROR;
	}
	return rec;
}
void facerec_config(facerec* rec, unsigned long size, double padding, int jittering) {
	FaceRec* cls = (FaceRec*)(rec->cls);
	cls->Config(size,padding,jittering);
}

faceret* facerec_recognize(facerec* rec, const uint8_t* img_data, int len, int max_faces,int type) {
	faceret* ret = (faceret*)calloc(1, sizeof(faceret));
	FaceRec* cls = (FaceRec*)(rec->cls);
	matrix<rgb_pixel> img;
	std::vector<rectangle> rects;
	std::vector<descriptor> descrs;
	std::vector<full_object_detection> shapes;

	try {
		// TODO(Kagami): Support more file types?
		load_mem_jpeg(img, img_data, len);
		std::tie(rects, descrs, shapes) = cls->Recognize(img, max_faces,type);
	} catch(image_load_error& e) {
		ret->err_str = strdup(e.what());
		ret->err_code = IMAGE_LOAD_ERROR;
		return ret;
	} catch (std::exception& e) {
		ret->err_str = strdup(e.what());
		ret->err_code = UNKNOWN_ERROR;
		return ret;
	}
	ret->num_faces = descrs.size();

	if (ret->num_faces == 0)
		return ret;
	ret->rectangles = (long*)malloc(ret->num_faces * RECT_SIZE);
	for (int i = 0; i < ret->num_faces; i++) {
		long* dst = ret->rectangles + i * RECT_LEN;
		dst[0] = rects[i].left();
		dst[1] = rects[i].top();
		dst[2] = rects[i].right();
		dst[3] = rects[i].bottom();
	}
	ret->descriptors = (float*)malloc(ret->num_faces * DESCR_SIZE);
	for (int i = 0; i < ret->num_faces; i++) {
		void* dst = (uint8_t*)(ret->descriptors) + i * DESCR_SIZE;
		void* src = (void*)&descrs[i](0,0);
		memcpy(dst, src, DESCR_SIZE);
	}
	ret->num_shapes = shapes[0].num_parts();
	ret->shapes = (long*)malloc(ret->num_faces * ret->num_shapes * SHAPE_SIZE);
	for (int i = 0; i < ret->num_faces; i++) {
		long* dst = ret->shapes + i * ret->num_shapes * SHAPE_LEN;
		const auto& shape = shapes[i];
		for (int j = 0; j < ret->num_shapes; j++) {
			dst[j*SHAPE_LEN] = shape.part(j).x();
			dst[j*SHAPE_LEN+1] = shape.part(j).y();
		}
	}
	return ret;
}

void facerec_set_samples(
	facerec* rec,
	const float* c_samples,
	const int32_t* c_cats,
	int len
) {
	FaceRec* cls = (FaceRec*)(rec->cls);
	std::vector<descriptor> samples;
	samples.reserve(len);
	for (int i = 0; i < len; i++) {
		descriptor sample = mat(c_samples + i*DESCR_LEN, DESCR_LEN, 1);
		samples.push_back(std::move(sample));
	}
	std::vector<int> cats(c_cats, c_cats + len);
	cls->SetSamples(std::move(samples), std::move(cats));
}

int facerec_classify(facerec* rec, const float* c_test_sample, float tolerance) {
	FaceRec* cls = (FaceRec*)(rec->cls);
	descriptor test_sample = mat(c_test_sample, DESCR_LEN, 1);
	return cls->Classify(test_sample, tolerance);
}

void facerec_free(facerec* rec) {
	if (rec) {
		if (rec->cls) {
			FaceRec* cls = (FaceRec*)(rec->cls);
			delete cls;
			rec->cls = NULL;
		}
		free(rec);
	}
}

static std::vector<matrix<rgb_pixel>> jitter_image(
    const matrix<rgb_pixel>& img,
    int count
)
{
    // All this function does is make count copies of img, all slightly jittered by being
    // zoomed, rotated, and translated a little bit differently. They are also randomly
    // mirrored left to right.
    thread_local dlib::rand rnd;

    std::vector<matrix<rgb_pixel>> crops;
    for (int i = 0; i < count; ++i)
        crops.push_back(jitter_image(img,rnd));

    return crops;
}


/***************************************************************************
 * Copyright 1998-2018 by authors (see AUTHORS.txt)                        *
 *                                                                         *
 *   This file is part of LuxCoreRender.                                   *
 *                                                                         *
 * 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.                                          *
 ***************************************************************************/

#include <boost/format.hpp>

#include "slg/lights/projectionlight.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// ProjectionLight
//------------------------------------------------------------------------------

ProjectionLight::ProjectionLight() : color(1.f), power(0.f), efficency(0.f),
		localPos(), localTarget(0.f, 0.f, 1.f),
		fov(45.f), imageMap(NULL) {
}

ProjectionLight::~ProjectionLight() {
}

void ProjectionLight::Preprocess() {
	const Vector dir = Normalize(localTarget - localPos);
	Vector du, dv;
	CoordinateSystem(dir, &du, &dv);
	const Transform dirToZ(Matrix4x4(
			du.x, du.y, du.z, 0.f,
			dv.x, dv.y, dv.z, 0.f,
			dir.x, dir.y, dir.z, 0.f,
			0.f, 0.f, 0.f, 1.f));
	alignedLight2World = lightToWorld *
			Translate(Vector(localPos.x, localPos.y, localPos.z)) /
			dirToZ;

	absolutePos = alignedLight2World * Point();
	lightNormal = Normalize(alignedLight2World * Normal(0.f, 0.f, 1.f));

	const float aspect = imageMap ? (imageMap->GetWidth() / (float)imageMap->GetHeight()) : 1.f;
	if (aspect > 1.f)  {
		screenX0 = -aspect;
		screenX1 = aspect;
		screenY0 = -1.f;
		screenY1 = 1.f;
	} else {
		screenX0 = -1.f;
		screenX1 = 1.f;
		screenY0 = -1.f / aspect;
		screenY1 = 1.f / aspect;
	}

	const float hither = DEFAULT_EPSILON_STATIC;
	const float yon = 1e30f;
	lightProjection = Perspective(fov, hither, yon);

	// Compute cosine of cone surrounding projection directions
	const float opposite = tanf(Radians(fov) / 2.f);
	const float tanDiag = opposite * sqrtf(1.f + 1.f / (aspect * aspect));
	cosTotalWidth = cosf(atanf(tanDiag));
	if (aspect <= 1.f)
		area = 4.f * opposite * opposite / aspect;
	else
		area = 4.f * opposite * opposite * aspect;

	emittedFactor = gain * color * (power * efficency /
			(2.f * M_PI * (imageMap ? imageMap->GetSpectrumMeanY() : 1.f) *
				(1.f - .5f * (1.f - cosTotalWidth))));
	if (emittedFactor.Black() || emittedFactor.IsInf() || emittedFactor.IsNaN())
		emittedFactor = gain * color;
}

void ProjectionLight::GetPreprocessedData(float *emittedFactorData, float *absolutePosData, float *lightNormalData,
		float *screenX0Data, float *screenX1Data, float *screenY0Data, float *screenY1Data,
		const luxrays::Transform **alignedLight2WorldData, const luxrays::Transform **lightProjectionData) const {
	if (emittedFactorData) {
		emittedFactorData[0] = emittedFactor.c[0];
		emittedFactorData[1] = emittedFactor.c[1];
		emittedFactorData[2] = emittedFactor.c[2];
	}

	if (absolutePosData) {
		absolutePosData[0] = absolutePos.x;
		absolutePosData[1] = absolutePos.y;
		absolutePosData[2] = absolutePos.z;
	}

	if (lightNormalData) {
		lightNormalData[0] = lightNormal.x;
		lightNormalData[1] = lightNormal.y;
		lightNormalData[2] = lightNormal.z;
	}

	if (screenX0Data)
		*screenX0Data = screenX0;
	if (screenX1Data)
		*screenX1Data = screenX1;
	if (screenY0Data)
		*screenY0Data = screenY0;
	if (screenY1Data)
		*screenY1Data = screenY1;

	if (alignedLight2WorldData)
		*alignedLight2WorldData = &alignedLight2World;
	if (lightProjectionData)
		*lightProjectionData = &lightProjection;
}

float ProjectionLight::GetPower(const Scene &scene) const {
	return emittedFactor.Y() *
			(imageMap ? imageMap->GetSpectrumMeanY() : 1.f) *
			2.f * M_PI * (1.f - cosTotalWidth);
}

Spectrum ProjectionLight::Emit(const Scene &scene,
		const float u0, const float u1, const float u2, const float u3, const float passThroughEvent,
		Point *orig, Vector *dir,
		float *emissionPdfW, float *directPdfA, float *cosThetaAtLight) const {
	*orig = absolutePos;
	const Point ps = Inverse(lightProjection) *
		Point(u0 * (screenX1 - screenX0) + screenX0, u1 * (screenY1 - screenY0) + screenY0, 0.f);
	*dir = Normalize(alignedLight2World * Vector(ps.x, ps.y, ps.z));
	const float cos = Dot(*dir, lightNormal);
	const float cos2 = cos * cos;
	*emissionPdfW = 1.f / (area * cos2 * cos);

	if (directPdfA)
		*directPdfA = 1.f;
	if (cosThetaAtLight)
		*cosThetaAtLight = 1.f;

	Spectrum c = emittedFactor;
	if (imageMap)
		c *= imageMap->GetSpectrum(UV(u0, u1));

	return c;
}

Spectrum ProjectionLight::Illuminate(const Scene &scene, const Point &p,
		const float u0, const float u1, const float passThroughEvent,
        Vector *dir, float *distance, float *directPdfW,
		float *emissionPdfW, float *cosThetaAtLight) const {
	const Vector toLight(absolutePos - p);
	const float distanceSquared = toLight.LengthSquared();
	*distance = sqrtf(distanceSquared);
	*dir = toLight / *distance;

	// Check the side
	if (Dot(-(*dir), lightNormal) < 0.f)
		return Spectrum();

	// Check if the point is inside the image plane
	const Vector localFromLight = Normalize(Inverse(alignedLight2World) * (-(*dir)));
	const Point p0 = lightProjection * Point(localFromLight.x, localFromLight.y, localFromLight.z);
	if ((p0.x < screenX0) || (p0.x >= screenX1) || (p0.y < screenY0) || (p0.y >= screenY1))
		return Spectrum();

	*directPdfW = distanceSquared;

	if (cosThetaAtLight)
		*cosThetaAtLight = 1.f;

	if (emissionPdfW)
		*emissionPdfW = 0.f;

	Spectrum c = emittedFactor;
	if (imageMap) {
		const float u = (p0.x - screenX0) / (screenX1 - screenX0);
		const float v = (p0.y - screenY0) / (screenY1 - screenY0);
		c *= imageMap->GetSpectrum(UV(u, v));
	}

	return c;
}

bool ProjectionLight::IsAlwaysInShadow(const Scene &scene,
			const luxrays::Point &p, const luxrays::Normal &n) const {
	const Vector toLight(absolutePos - p);
	const float distance = toLight.Length();
	const Vector dir = toLight / distance;

	// Check the side
	return (Dot(-dir, lightNormal) < 0.f);
}

Properties ProjectionLight::ToProperties(const ImageMapCache &imgMapCache, const bool useRealFileName) const {
	const string prefix = "scene.lights." + GetName();
	Properties props = NotIntersectableLightSource::ToProperties(imgMapCache, useRealFileName);

	props.Set(Property(prefix + ".type")("projection"));
	props.Set(Property(prefix + ".color")(color));
	props.Set(Property(prefix + ".power")(power));
	props.Set(Property(prefix + ".efficency")(efficency));
	props.Set(Property(prefix + ".position")(localPos));
	props.Set(Property(prefix + ".target")(localTarget));
	props.Set(Property(prefix + ".fov")(fov));

	const string fileName = useRealFileName ?
		imageMap->GetName() : imgMapCache.GetSequenceFileName(imageMap);
	props.Set(Property(prefix + ".mapfile")(fileName));
	props.Set(imageMap->ToProperties(prefix, false));

	return props;
}


//  Copyright (c) 2006-2009, Bernhard Reiter
//
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

#ifndef LIBS_TREE_TEST_TEST_DATA_HPP
#define LIBS_TREE_TEST_TEST_DATA_HPP

#include <boost/tree/algorithm.hpp>

#include <boost/multi_index_container.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/member.hpp>

struct test_data {
    typedef std::size_t size_type;
    typedef int value_type;

    size_type pos_level;
    size_type pre;
    size_type in;
    size_type post;

    value_type val;

    test_data(size_type lop, size_type _pre, size_type _in, size_type _post, int v)
    : pos_level (lop)
    , pre(_pre), in(_in), post(_post)
    , val(v)
    {}

    size_type const& get(boost::tree::preorder) const { return pre; }
    size_type const& get(boost::tree::inorder) const { return in; }
    size_type const& get(boost::tree::postorder) const { return post; }
};

template <class Cont>
void mock_cursor_data(Cont& data)
{
    data.insert(test_data( 0, 0, 5,10, 8));
    data.insert(test_data( 1, 1, 1, 4, 3));
    data.insert(test_data( 3, 2, 0, 0, 1));
    data.insert(test_data( 4, 3, 3, 3, 6));
    data.insert(test_data( 9, 4, 2, 1, 4));
    data.insert(test_data(10, 5, 4, 2, 7));
    data.insert(test_data( 2, 6, 6, 9,10));
    data.insert(test_data( 6, 7,10, 8,14));
    data.insert(test_data(13, 8, 9, 7,13));
    data.insert(test_data(27, 9, 7, 6,11));
    data.insert(test_data(56,10, 8, 5,12));
}

using boost::multi_index::indexed_by;
using boost::multi_index::member;
using boost::multi_index::ordered_unique;
using boost::multi_index::tag;

typedef boost::multi_index::multi_index_container<
  test_data,
  indexed_by<
    ordered_unique< member<test_data,test_data::size_type,&test_data::pos_level> >,

    ordered_unique<tag<boost::tree::preorder>,member<test_data,test_data::size_type,&test_data::pre> >,
    ordered_unique<tag<boost::tree::inorder>,member<test_data,test_data::size_type,&test_data::in> >,
    ordered_unique<tag<boost::tree::postorder>,member<test_data,test_data::size_type,&test_data::post> >
  >
> test_data_set;

#endif // LIBS_TREE_TEST_TEST_DATA_HPP


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

#include "bempp/common/config_ahmed.hpp"

#ifdef WITH_AHMED

#include "potential_operator_aca_assembly_helper.hpp"

#include "evaluation_options.hpp"
#include "discrete_boundary_operator.hpp"
#include "local_dof_lists_cache.hpp"
#include "component_lists_cache.hpp"

#include "../common/boost_make_shared_fwd.hpp"
#include "../common/multidimensional_arrays.hpp"
#include "../fiber/explicit_instantiation.hpp"
#include "../fiber/local_assembler_for_potential_operators.hpp"
#include "../fiber/types.hpp"
#include "../grid/grid_view.hpp"
#include "../space/space.hpp"

#include "ahmed_aux.hpp"
#include "ahmed_complex.hpp"

#include <map>
#include <set>
#include <utility>
#include <cstdio>
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>

namespace Bempp
{

template <typename BasisFunctionType, typename ResultType>
PotentialOperatorAcaAssemblyHelper<BasisFunctionType, ResultType>::
PotentialOperatorAcaAssemblyHelper(
        const arma::Mat<CoordinateType>& points,
        const Space<BasisFunctionType>& trialSpace,
        const std::vector<unsigned int>& p2oPoints,
        const std::vector<unsigned int>& p2oTrialDofs,
        const std::vector<LocalAssembler*>& assemblers,
        const std::vector<ResultType>& termMultipliers,
        const EvaluationOptions& options) :
    m_points(points), m_trialSpace(trialSpace),
    m_p2oPoints(p2oPoints), m_p2oTrialDofs(p2oTrialDofs),
    m_assemblers(assemblers),
    m_termMultipliers(termMultipliers),
    m_options(options),
    m_indexWithGlobalDofs(m_options.acaOptions().globalAssemblyBeforeCompression),
    m_trialDofListsCache(new LocalDofListsCache<BasisFunctionType>(
                            m_trialSpace, m_p2oTrialDofs, m_indexWithGlobalDofs))
{
    if (assemblers.empty())
        throw std::invalid_argument(
                "PotentialOperatorAcaAssemblyHelper::"
                "PotentialOperatorAcaAssemblyHelper(): "
                "the 'assemblers' vector must not be null");
    if (assemblers.size() != termMultipliers.size())
        throw std::invalid_argument(
                "PotentialOperatorAcaAssemblyHelper::"
                "PotentialOperatorAcaAssemblyHelper(): "
                "the 'assemblers' and 'termMultipliers' vectors must have the "
                "same length");
    for (size_t i = 0; i < assemblers.size(); ++i)
        if (!assemblers[i])
            throw std::invalid_argument(
                    "PotentialOperatorAcaAssemblyHelper::"
                    "PotentialOperatorAcaAssemblyHelper(): "
                    "no elements of the 'assemblers' vector may be null");
    m_componentCount = assemblers[0]->resultDimension();
    for (size_t i = 1; i < assemblers.size(); ++i)
        if (assemblers[i]->resultDimension() != m_componentCount)
            throw std::invalid_argument(
                    "PotentialOperatorAcaAssemblyHelper::"
                    "PotentialOperatorAcaAssemblyHelper(): "
                    "all assemblers must produce results with the same number "
                    "of components");
    m_componentListsCache.reset(
                new ComponentListsCache(m_p2oPoints, m_componentCount));
    resetAccessedEntryCount();
}

template <typename BasisFunctionType, typename ResultType>
typename PotentialOperatorAcaAssemblyHelper<BasisFunctionType, ResultType>::MagnitudeType
PotentialOperatorAcaAssemblyHelper<BasisFunctionType, ResultType>::estimateMinimumDistance(
        const cluster* c1, const cluster* c2) const
{
    typedef typename Fiber::ScalarTraits<BasisFunctionType>::RealType CoordinateType;
    typedef AhmedDofWrapper<CoordinateType> AhmedDofType;
    typedef ExtendedBemCluster<AhmedDofType> AhmedBemCluster;

    AhmedBemCluster* cluster1 =
        const_cast<AhmedBemCluster*>(// AHMED is not const-correct
            dynamic_cast<const AhmedBemCluster*>(c1));
    AhmedBemCluster* cluster2 =
        const_cast<AhmedBemCluster*>(// AHMED is not const-correct
            dynamic_cast<const AhmedBemCluster*>(c2));
    // Lower bound on the minimum distance between elements from the two clusters
    CoordinateType minDist = -1.; // negative, read: unknown
    if (cluster1 && cluster2) {
        // both getdiam2() and dist2() are effectively const, but not declared so
        // in AHMED
        const double diam1 = sqrt(cluster1->getdiam2());
        const double diam2 = sqrt(cluster2->getdiam2());
        const double dist = sqrt(cluster1->dist2(cluster2));
        minDist = std::max(0., dist - (diam1 + diam2) / 2.);
    }
    // else
        // std::cout << "Warning: clusters not available" << std::endl;
    return minDist;
}

template <typename BasisFunctionType, typename ResultType>
void PotentialOperatorAcaAssemblyHelper<BasisFunctionType, ResultType>::cmpbl(
        unsigned b1, unsigned n1, unsigned b2, unsigned n2,
        AhmedResultType* ahmedData,
        const cluster* c1, const cluster* c2, bool countAccessedEntries) const
{
//    std::cout << "\nRequested block: (" << b1 << ", " << n1 << "; "
//              << b2 << ", " << n2 << ")" << std::endl;
    if (countAccessedEntries)
        m_accessedEntryCount += n1 * n2;

    typedef typename Fiber::ScalarTraits<BasisFunctionType>::RealType CoordinateType;
    typedef AhmedDofWrapper<CoordinateType> AhmedDofType;
    typedef ExtendedBemCluster<AhmedDofType> AhmedBemCluster;

    AhmedBemCluster* cluster1 =
        const_cast<AhmedBemCluster*>(// AHMED is not const-correct
            dynamic_cast<const AhmedBemCluster*>(c1));
    AhmedBemCluster* cluster2 =
        const_cast<AhmedBemCluster*>(// AHMED is not const-correct
            dynamic_cast<const AhmedBemCluster*>(c2));
    // Lower bound on the minimum distance between elements from the two clusters
    CoordinateType minDist = -1.; // negative, read: unknown
    if (cluster1 && cluster2) {
        // both getdiam2() and dist2() are effectively const, but not declared so
        // in AHMED
        const double diam1 = sqrt(cluster1->getdiam2());
        const double diam2 = sqrt(cluster2->getdiam2());
        const double dist = sqrt(cluster1->dist2(cluster2));
        minDist = std::max(0., dist - (diam1 + diam2) / 2.);
    }
    // else
        // std::cout << "Warning: clusters not available" << std::endl;
    // std::cout << "minDist: " << minDist << std::endl;

    // This is a non-op for real types. For complex types, it converts a pointer
    // to Ahmed's scomp (resp. dcomp) to a pointer to std::complex<float>
    // (resp. std::complex<double>), which should be perfectly safe since these
    // types have the same binary representation.
    ResultType* data = reinterpret_cast<ResultType*>(ahmedData);

    // Convert AHMED matrix indices into point and DOF indices
    shared_ptr<const ComponentLists> componentLists =
            m_componentListsCache->get(b1, n1);
    shared_ptr<const LocalDofLists<BasisFunctionType> > trialDofLists =
            m_trialDofListsCache->get(b2, n2);

    // Necessary points
    const std::vector<int>& pointIndices =
            componentLists->pointIndices;
    // Necessary components at each point
    const std::vector<std::vector<int> >& componentIndices =
            componentLists->componentIndices;

    typedef typename LocalDofLists<BasisFunctionType>::DofIndex DofIndex;
    // Necessary elements
    const std::vector<int>& trialElementIndices =
            trialDofLists->elementIndices;
    // Necessary local dof indices in each element
    const std::vector<std::vector<LocalDofIndex> >& trialLocalDofs =
            trialDofLists->localDofIndices;
    // Weights of local dofs in each element
    const std::vector<std::vector<BasisFunctionType> >& trialLocalDofWeights =
            trialDofLists->localDofWeights;
    for (size_t i = 0; i < trialLocalDofWeights.size(); ++i)
        for (size_t j = 0; j < trialLocalDofWeights[i].size(); ++j)
            assert(std::abs(trialLocalDofWeights[i][j]) > 0.);
    // Corresponding row and column indices in the matrix to be calculated
    // and stored in ahmedData
    const std::vector<std::vector<int> >& blockRows =
            componentLists->arrayIndices;
    const std::vector<std::vector<int> >& blockCols =
            trialDofLists->arrayIndices;

    arma::Mat<ResultType> result(data, n1, n2, false /*copy_aux_mem*/,
                                 true /*strict*/);
    result.fill(0.);

    if (n2 == 1) {
        // Only one column of the block needed. This means that we need only
        // one local DOF from just one or a few trialElements. Evaluate the
        // local potential operator for one local trial DOF at a time.

        // indices: vector: point index; matrix: component, dof
        std::vector<arma::Mat<ResultType> > localResult;
        for (size_t nTrialElem = 0;
             nTrialElem < trialElementIndices.size();
             ++nTrialElem) {
            const int activeTrialElementIndex = trialElementIndices[nTrialElem];

            // The body of this loop will very probably only run once (single
            // local DOF per trial element)
            for (size_t nTrialDof = 0;
                 nTrialDof < trialLocalDofs[nTrialElem].size();
                 ++nTrialDof) {
                LocalDofIndex activeTrialLocalDof =
                        trialLocalDofs[nTrialElem][nTrialDof];
                BasisFunctionType activeTrialLocalDofWeight =
                        trialLocalDofWeights[nTrialElem][nTrialDof];
                for (size_t nTerm = 0; nTerm < m_assemblers.size(); ++nTerm) {
                    m_assemblers[nTerm]->evaluateLocalContributions(
                                pointIndices,
                                activeTrialElementIndex, activeTrialLocalDof,
                                localResult, minDist);
                    for (size_t nPoint = 0;
                         nPoint < pointIndices.size();
                         ++nPoint)
                        for (size_t nComponent = 0;
                             nComponent < componentIndices[nPoint].size();
                             ++nComponent)
                            result(blockRows[nPoint][nComponent], 0) +=
                                    m_termMultipliers[nTerm] *
                                    activeTrialLocalDofWeight *
                                    localResult[nPoint](
                                        componentIndices[nPoint][nComponent], 0);
                }
            }
        }
    }
    else if (n1 == 1) {
        // Only one row of the block needed. This means that we need to
        // evaluate a single component of the local potential operator at
        // a single point.
        assert(pointIndices.size() == 1);
        assert(componentIndices.size() == 1);
        assert(componentIndices[0].size() == 1);

        // indices: vector: trial element; matrix: component, dof
        std::vector<arma::Mat<ResultType> > localResult;
        for (size_t nTerm = 0; nTerm < m_assemblers.size(); ++nTerm) {
            m_assemblers[nTerm]->evaluateLocalContributions(
                        pointIndices[0], componentIndices[0][0],
                        trialElementIndices, localResult, minDist);
            for (size_t nTrialElem = 0;
                 nTrialElem < trialElementIndices.size();
                 ++nTrialElem)
                for (size_t nTrialDof = 0;
                     nTrialDof < trialLocalDofs[nTrialElem].size();
                     ++nTrialDof)
                    result(0, blockCols[nTrialElem][nTrialDof]) +=
                            m_termMultipliers[nTerm] *
                            trialLocalDofWeights[nTrialElem][nTrialDof] *
                            localResult[nTrialElem](
                                0, trialLocalDofs[nTrialElem][nTrialDof]);
        }
    }
    else { // a "fat" block
        // The whole block or its submatrix needed. This means that we are
        // likely to need all or almost all local DOFs from most elements.
        // Evaluate the full local weak form for each pair of test and trial
        // elements and then select the entries that we need.

        Fiber::_2dArray<arma::Mat<ResultType> > localResult;
        for (size_t nTerm = 0; nTerm < m_assemblers.size(); ++nTerm) {
            m_assemblers[nTerm]->evaluateLocalContributions(
                        pointIndices, trialElementIndices,
                        localResult, minDist);
            for (size_t nTrialElem = 0;
                 nTrialElem < trialElementIndices.size();
                 ++nTrialElem)
                for (size_t nTrialDof = 0;
                     nTrialDof < trialLocalDofs[nTrialElem].size();
                     ++nTrialDof)
                    for (size_t nPoint = 0;
                         nPoint < pointIndices.size();
                         ++nPoint)
                        for (size_t nComponent = 0;
                             nComponent < componentIndices[nPoint].size();
                             ++nComponent)
                            result(blockRows[nPoint][nComponent],
                                   blockCols[nTrialElem][nTrialDof]) +=
                                    m_termMultipliers[nTerm] *
                                    trialLocalDofWeights[nTrialElem][nTrialDof] *
                                    localResult(nPoint, nTrialElem)
                                    (componentIndices[nPoint][nComponent],
                                     trialLocalDofs[nTrialElem][nTrialDof]);
        }
    }
}

template <typename BasisFunctionType, typename ResultType>
void PotentialOperatorAcaAssemblyHelper<BasisFunctionType, ResultType>::cmpblsym(
        unsigned b1, unsigned n1, AhmedResultType* ahmedData,
        const cluster* c1, bool countAccessedEntries) const
{
    if (countAccessedEntries)
        m_accessedEntryCount += n1 * n1;

    // Calculate results as usual (without taking symmetry into account)
    arma::Mat<ResultType> block(n1, n1);
    cmpbl(b1, n1, b1, n1, ahmedCast(block.memptr()), c1, c1);

    // and now store the upper part of the matrix in the memory block
    // provided by Ahmed
    for (size_t col = 0; col < n1; ++col)
        for (size_t row = 0; row <= col; ++row)
            *ahmedData++ = ahmedCast(block(row, col));
}

template <typename BasisFunctionType, typename ResultType>
typename PotentialOperatorAcaAssemblyHelper<BasisFunctionType, ResultType>::MagnitudeType
PotentialOperatorAcaAssemblyHelper<BasisFunctionType, ResultType>::scale(
        unsigned b1, unsigned n1, unsigned b2, unsigned n2,
        const cluster* c1, const cluster* c2) const
{
    //    return m_options.acaOptions().scaling;
    typedef typename Fiber::ScalarTraits<BasisFunctionType>::RealType CoordinateType;
    typedef AhmedDofWrapper<CoordinateType> AhmedDofType;
    typedef ExtendedBemCluster<AhmedDofType> AhmedBemCluster;

    AhmedBemCluster* cluster1 =
            const_cast<AhmedBemCluster*>(// AHMED is not const-correct
                                         dynamic_cast<const AhmedBemCluster*>(c1));
    AhmedBemCluster* cluster2 =
            const_cast<AhmedBemCluster*>(// AHMED is not const-correct
                                         dynamic_cast<const AhmedBemCluster*>(c2));
    if (cluster1 && cluster2) {
        // both getdiam2() and dist2() are effectively const, but not declared so
        // in AHMED
        const CoordinateType dist = sqrt(cluster1->dist2(cluster2));
        MagnitudeType result = 0.;
        for (size_t nTerm = 0; nTerm < m_assemblers.size(); ++nTerm)
            result = std::max(result,
                              m_assemblers[nTerm]->estimateRelativeScale(dist));
        return result;
    }
    else
        return m_options.acaOptions().scaling;
}

template <typename BasisFunctionType, typename ResultType>
typename PotentialOperatorAcaAssemblyHelper<BasisFunctionType, ResultType>::MagnitudeType
PotentialOperatorAcaAssemblyHelper<BasisFunctionType, ResultType>::relativeScale(
        unsigned b1, unsigned n1, unsigned b2, unsigned n2,
        const cluster* c1, const cluster* c2) const
{
    const CoordinateType minDist = estimateMinimumDistance(c1, c2);
    if (minDist < 0)
        return 1.;
    MagnitudeType result = 0.;
    for (size_t nTerm = 0; nTerm < m_assemblers.size(); ++nTerm)
        result = std::max(result,
                          m_assemblers[nTerm]->estimateRelativeScale(minDist));
    return result;
}

template <typename BasisFunctionType, typename ResultType>
size_t
PotentialOperatorAcaAssemblyHelper<BasisFunctionType, ResultType>::
accessedEntryCount() const
{
    return m_accessedEntryCount;
}

template <typename BasisFunctionType, typename ResultType>
void
PotentialOperatorAcaAssemblyHelper<BasisFunctionType, ResultType>::
resetAccessedEntryCount()
{
    m_accessedEntryCount = 0;
}

// Explicit instantiations

FIBER_INSTANTIATE_CLASS_TEMPLATED_ON_BASIS_AND_RESULT(PotentialOperatorAcaAssemblyHelper);

}

#endif // WITH_AHMED


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform 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.
 */

#include <ff_common/init.h>
#include <graph_bag/sparse_mapping_pose_adder.h>
#include <localization_common/logger.h>
#include <localization_common/utilities.h>

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

#include <gtsam/geometry/Pose3.h>

namespace po = boost::program_options;
namespace lc = localization_common;

int main(int argc, char** argv) {
  std::string robot_config_file;
  std::string world;
  po::options_description desc(
    "Adds sparse mapping poses to a new bag file using sparse mapping feature messages and body_T_nav_cam extrinsics.");
  desc.add_options()("help", "produce help message")("bagfile", po::value<std::string>()->required(), "Bagfile")(
    "config-path,c", po::value<std::string>()->required(), "Config path")(
    "robot-config-file,r", po::value<std::string>(&robot_config_file)->default_value("config/robots/bumble.config"),
    "Robot config file")("world,w", po::value<std::string>(&world)->default_value("iss"), "World name");
  po::positional_options_description p;
  p.add("bagfile", 1);
  p.add("config-path", 1);
  po::variables_map vm;
  try {
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
    po::notify(vm);
  } catch (std::exception& e) {
    std::cerr << "Error: " << e.what() << "\n";
    return 1;
  }

  if (vm.count("help")) {
    std::cout << desc << "\n";
    return 1;
  }

  const std::string input_bag = vm["bagfile"].as<std::string>();
  const std::string config_path = vm["config-path"].as<std::string>();

  // Only pass program name to free flyer so that boost command line options
  // are ignored when parsing gflags.
  int ff_argc = 1;
  ff_common::InitFreeFlyerApplication(&ff_argc, &argv);

  if (!boost::filesystem::exists(input_bag)) {
    LogFatal("Bagfile " << input_bag << " not found.");
  }

  boost::filesystem::path input_bag_path(input_bag);
  boost::filesystem::path output_bag_path =
    input_bag_path.parent_path() /
    boost::filesystem::path(input_bag_path.stem().string() + "_with_sparse_mapping_poses.bag");
  lc::SetEnvironmentConfigs(config_path, world, robot_config_file);
  config_reader::ConfigReader config;
  config.AddFile("geometry.config");
  if (!config.ReadFiles()) {
    LogFatal("Failed to read config files.");
  }

  const gtsam::Pose3 body_T_nav_cam = lc::LoadTransform(config, "nav_cam_transform");
  graph_bag::SparseMappingPoseAdder sparse_mapping_pose_adder(input_bag, output_bag_path.string(),
                                                              body_T_nav_cam.inverse());
  sparse_mapping_pose_adder.AddPoses();
}


/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkPBGLShortestPaths.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/*-------------------------------------------------------------------------
  Copyright 2008 Sandia Corporation.
  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
  the U.S. Government retains certain rights in this software.
-------------------------------------------------------------------------*/
/*
 * Copyright (C) 2008 The Trustees of Indiana University.
 * Use, modification and distribution is subject to the Boost Software
 * License, Version 1.0. (See http://www.boost.org/LICENSE_1_0.txt)
 */
#include "vtkPBGLShortestPaths.h"

#include "vtkCellArray.h"
#include "vtkCellData.h"
#include "vtkConvertSelection.h"
#include "vtkDataArray.h"
#include "vtkDirectedGraph.h"
#include "vtkDistributedGraphHelper.h"
#include "vtkFloatArray.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkMath.h"
#include "vtkObjectFactory.h"
#include "vtkPBGLDistributedGraphHelper.h"
#include "vtkPBGLGraphAdapter.h"
#include "vtkPointData.h"
#include "vtkSelection.h"
#include "vtkSelectionNode.h"
#include "vtkSmartPointer.h"
#include "vtkStreamingDemandDrivenPipeline.h"
#include "vtkStringArray.h"
#include "vtkType.h"
#include "vtkUndirectedGraph.h"
#include "vtkVertexListIterator.h"

#include <boost/graph/use_mpi.hpp>

#include <boost/graph/distributed/delta_stepping_shortest_paths.hpp>
#include <boost/graph/parallel/algorithm.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/vector_property_map.hpp>

#include <vtksys/stl/utility> // for pair

using namespace boost;

vtkStandardNewMacro(vtkPBGLShortestPaths);

// Function object used to reduce (vertex, distance) pairs to find
// the furthest vertex. This ordering favors vertices on processors
// with a lower rank. Used only for the Parallel BGL shortest-paths.
class furthest_vertex_double
{
public:
  furthest_vertex_double() : graph(0) { }

  furthest_vertex_double(vtkGraph *g) : graph(g) { }

  vtkstd::pair<vtkIdType, double>
  operator()(vtkstd::pair<vtkIdType, double> x,
             vtkstd::pair<vtkIdType, double> y) const
  {
    vtkDistributedGraphHelper *helper = graph->GetDistributedGraphHelper();
    if (x.second > y.second
        || (x.second == y.second
            && helper->GetVertexOwner(x.first) < helper->GetVertexOwner(y.first))
        || (x.second == y.second
            && helper->GetVertexOwner(x.first) == helper->GetVertexOwner(y.first)
            && helper->GetVertexIndex(x.first) < helper->GetVertexIndex(y.first)))
      {
      return x;
      }
    else
      {
      return y;
      }
  }

private:
  vtkGraph *graph;
};

// Constructor/Destructor
vtkPBGLShortestPaths::vtkPBGLShortestPaths()
{
  // Default values for the origin vertex
  this->OriginVertexIndex = 0;
  this->InputArrayName = 0;
  this->EdgeWeightArrayName = 0;
  this->Delta = 0.0;
  this->PredecessorArrayName = 0;
  this->PathLengthArrayName = 0;
  this->OutputSelectionType = 0;
  this->SetOutputSelectionType("MAX_DIST_FROM_ROOT");
  this->OriginValue = -1;
  this->OutputSelection = false;
  this->OriginFromSelection = false;
  this->UseUniformEdgeWeights = false;
  this->SetNumberOfInputPorts(2);
  this->SetNumberOfOutputPorts(2);
}

vtkPBGLShortestPaths::~vtkPBGLShortestPaths()
{
  this->SetInputArrayName(0);
  this->SetEdgeWeightArrayName(0);
  this->SetPredecessorArrayName(0);
  this->SetPathLengthArrayName(0);
}

void vtkPBGLShortestPaths::SetOriginSelection(vtkSelection* s)
{
  this->SetOriginSelectionConnection(s->GetProducerPort());
}

// Description:
// Set the index (into the vertex array) of the
// breadth first search 'origin' vertex.
void vtkPBGLShortestPaths::SetOriginVertex(vtkIdType index)
{
  this->OriginVertexIndex = index;
  this->InputArrayName = NULL; // Reset any origin set by another method
  this->Modified();
}

// Description:
// Set the breadth first search 'origin' vertex.
// This method is basically the same as above
// but allows the application to simply specify
// an array name and value, instead of having to
// know the specific index of the vertex.
void vtkPBGLShortestPaths::SetOriginVertex(
  vtkStdString arrayName, vtkVariant value)
{
  this->SetInputArrayName(arrayName);
  this->OriginValue = value;
  this->Modified();
}

void vtkPBGLShortestPaths::SetOriginVertexString(
  char* arrayName, char* value)
{
  this->SetOriginVertex(arrayName, value);
}

vtkIdType vtkPBGLShortestPaths::GetVertexIndex(
  vtkAbstractArray *abstract,vtkVariant value)
{

  // Okay now what type of array is it
  if (abstract->IsNumeric())
    {
    vtkDataArray *dataArray = vtkDataArray::SafeDownCast(abstract);
    int intValue = value.ToInt();
    for(int i=0; i<dataArray->GetNumberOfTuples(); ++i)
      {
      if (intValue == static_cast<int>(dataArray->GetTuple1(i)))
        {
        return i;
        }
      }
    }
  else
    {
    vtkStringArray *stringArray = vtkStringArray::SafeDownCast(abstract);
    vtkStdString stringValue(value.ToString());
    for(int i=0; i<stringArray->GetNumberOfTuples(); ++i)
      {
      if (stringValue == stringArray->GetValue(i))
        {
        return i;
        }
      }
    }

  // Failed
  vtkErrorMacro("Did not find a valid vertex index...");
  return 0;
}

int vtkPBGLShortestPaths::RequestData(
  vtkInformation *vtkNotUsed(request),
  vtkInformationVector **inputVector,
  vtkInformationVector *outputVector)
{
  // get the info objects
  vtkInformation *inInfo = inputVector[0]->GetInformationObject(0);
  vtkInformation *outInfo = outputVector->GetInformationObject(0);

  // get the input and output
  vtkGraph *input = vtkGraph::SafeDownCast(
      inInfo->Get(vtkDataObject::DATA_OBJECT()));
  vtkGraph *output = vtkGraph::SafeDownCast(
      outInfo->Get(vtkDataObject::DATA_OBJECT()));

  // Send the data to output.
  output->ShallowCopy(input);

  // Sanity check
  // The Boost BFS likes to crash on empty datasets
  if (input->GetNumberOfVertices() == 0)
    {
    //vtkWarningMacro("Empty input into " << this->GetClassName());
    return 1;
    }

  if (this->OriginFromSelection)
    {
    vtkSelection* selection = vtkSelection::GetData(inputVector[1], 0);
    if (selection == NULL)
      {
      vtkErrorMacro("OriginFromSelection set but selection input undefined.");
      return 0;
      }
    vtkSmartPointer<vtkIdTypeArray> idArr =
      vtkSmartPointer<vtkIdTypeArray>::New();
    vtkConvertSelection::GetSelectedVertices(selection, input, idArr);
    if (idArr->GetNumberOfTuples() == 0)
      {
      vtkErrorMacro("Origin selection is empty.");
      return 0;
      }
    this->OriginVertexIndex = idArr->GetValue(0);
    }
  else
    {
    // Now figure out the origin vertex of the
    // breadth first search
    if (this->InputArrayName)
      {
      vtkAbstractArray* abstract = input->GetVertexData()->GetAbstractArray(this->InputArrayName);

      // Does the array exist at all?
      if (abstract == NULL)
        {
        vtkErrorMacro("Could not find array named " << this->InputArrayName);
        return 0;
        }

      this->OriginVertexIndex = this->GetVertexIndex(abstract,this->OriginValue);
      }
    }

  vtkAbstractArray * abstractEdgeWeightArray = NULL;
  vtkDoubleArray   * edgeWeightArray = NULL;
  bool edgeWeightArrayIsTemporary = false;

  if(!this->UseUniformEdgeWeights)
    {
    // Retrieve the edge-weight array.
    if (!this->EdgeWeightArrayName)
      {
      vtkErrorMacro("Edge-weight array name is required");
      return 1;
      }
    abstractEdgeWeightArray = input->GetEdgeData()->GetAbstractArray(this->EdgeWeightArrayName);

    // Does the edge-weight array exist at all?
    if (abstractEdgeWeightArray == NULL)
      {
      vtkErrorMacro("Could not find edge-weight array named "
                    << this->EdgeWeightArrayName);
      return 1;
      }

    // Does the edge-weight array have enough values in it?
    if (abstractEdgeWeightArray->GetNumberOfTuples() < output->GetNumberOfEdges())
      {
      vtkErrorMacro("Edge-weight array named " << this->EdgeWeightArrayName
                  << " has too few values in it.");
      return 1;
      }

    edgeWeightArray = vtkDoubleArray::SafeDownCast(abstractEdgeWeightArray);
    if (edgeWeightArray == 0)
      {
      // Edge-weight array does not contain "double" values. We will
      // need to build a temporary array, or fail.
      if (abstractEdgeWeightArray->IsNumeric())
        {
        // Allocate a new edge-weight array.
        edgeWeightArray = vtkDoubleArray::New();
        edgeWeightArray->SetNumberOfTuples(output->GetNumberOfEdges());
        edgeWeightArrayIsTemporary = true;

        // Convert the values in the given array into doubles.
        for (vtkIdType i = 0; i < output->GetNumberOfEdges(); ++i)
          {
          vtkVariant value = abstractEdgeWeightArray->GetVariantValue(i);
          edgeWeightArray->SetTuple1(i, value.ToDouble());
          }
        }
      else
        {
        vtkErrorMacro("Edge-weight array named " << this->EdgeWeightArrayName
                      << " does not contain numeric values.")
        return 1;
        }
      }
    }
  else
    {
    // Create a default edge weight array with uniform edge weights.
    edgeWeightArray = vtkDoubleArray::New();
    edgeWeightArray->SetNumberOfTuples(output->GetNumberOfEdges());
    edgeWeightArrayIsTemporary = true;

    for(vtkIdType i=0; i<output->GetNumberOfEdges(); ++i)
      {
      edgeWeightArray->SetTuple1(i, 1.0);
      }
    }

  // Create the predecessor array
  vtkIdTypeArray* PredecessorArray = vtkIdTypeArray::New();
  if (this->PredecessorArrayName)
    {
    PredecessorArray->SetName(this->PredecessorArrayName);
    }
  else
    {
    PredecessorArray->SetName("Predecessor");
    }
  PredecessorArray->SetNumberOfTuples(output->GetNumberOfVertices());

  // Create the path-length array
  vtkDoubleArray* PathLengthArray = vtkDoubleArray::New();
  if (this->PathLengthArrayName)
    {
    PathLengthArray->SetName(this->PathLengthArrayName);
    }
  else
    {
    PathLengthArray->SetName("PathLength");
    }
  PathLengthArray->SetNumberOfTuples(output->GetNumberOfVertices());

  vtkDistributedGraphHelper *helper = output->GetDistributedGraphHelper();
  if (!helper)
    {
    vtkErrorMacro("Distributed vtkGraph is required.");
    return 1;
    }

  // We can only deal with Parallel BGL-distributed graphs.
  vtkPBGLDistributedGraphHelper *pbglHelper
    = vtkPBGLDistributedGraphHelper::SafeDownCast(helper);
  if (!pbglHelper)
    {
    vtkErrorMacro("Can only perform parallel shortest-paths on a Parallel BGL distributed graph");
    return 1;
    }

  int myRank = outInfo->Get(vtkStreamingDemandDrivenPipeline::UPDATE_PIECE_NUMBER());

  // Distributed predecessor map
  typedef vtkDistributedVertexPropertyMapType<vtkIdTypeArray>::type
    DistributedPredecessorMap;
  DistributedPredecessorMap predecessorMap
    = MakeDistributedVertexPropertyMap(output, PredecessorArray);

  // Distributed path-length map
  typedef vtkDistributedVertexPropertyMapType<vtkDoubleArray>::type
    DistributedPathLengthMap;
  DistributedPathLengthMap pathLengthMap
    = MakeDistributedVertexPropertyMap(output, PathLengthArray);

  // Distributed edge-weight map
  typedef vtkDistributedEdgePropertyMapType<vtkDoubleArray>::type
    DistributedEdgeWeightMap;
  DistributedEdgeWeightMap edgeWeightMap
    = MakeDistributedEdgePropertyMap(output, edgeWeightArray);

  if (vtkDirectedGraph::SafeDownCast(output))
    {
    vtkDirectedGraph *g = vtkDirectedGraph::SafeDownCast(output);
    if (this->Delta > 0.0)
      {
      boost::graph::distributed::delta_stepping_shortest_paths
        (g, this->OriginVertexIndex,
         predecessorMap, pathLengthMap, edgeWeightMap, this->Delta);
      }
    else
      {
      boost::graph::distributed::delta_stepping_shortest_paths
        (g, this->OriginVertexIndex,
         predecessorMap, pathLengthMap, edgeWeightMap);
      }
    }
  else
    {
    vtkUndirectedGraph *g = vtkUndirectedGraph::SafeDownCast(output);
    if (this->Delta > 0.0)
      {
      boost::graph::distributed::delta_stepping_shortest_paths
        (g, this->OriginVertexIndex,
         predecessorMap, pathLengthMap, edgeWeightMap, this->Delta);
      }
    else
      {
      boost::graph::distributed::delta_stepping_shortest_paths
        (g, this->OriginVertexIndex,
         predecessorMap, pathLengthMap, edgeWeightMap);
      }
    }

  // Since we know PredecessorArray will contain DistributedIds, we can flag
  // it by adding the key, DISTRIBUTEDIDS(), so that CollectGraph will know
  // to recalculate the values when collecting to 1 node.
  // This might also be used for repartitioning as well.
  PredecessorArray->GetInformation()->Set(
        vtkDistributedGraphHelper::DISTRIBUTEDVERTEXIDS(), 1);

  // Add output arrays to the output
  output->GetVertexData()->AddArray(PredecessorArray);
  PredecessorArray->Delete();
  output->GetVertexData()->AddArray(PathLengthArray);
  PathLengthArray->Delete();

  if (edgeWeightArrayIsTemporary)
    {
    edgeWeightArray->Delete();
    }

  if (this->OutputSelection)
    {
    vtkSelection* sel = vtkSelection::GetData(outputVector, 1);
    vtkSmartPointer<vtkIdTypeArray> ids =
      vtkSmartPointer<vtkIdTypeArray>::New();
    vtkSmartPointer<vtkSelectionNode> node =
      vtkSmartPointer<vtkSelectionNode>::New();

    // Set the output based on the output selection type
    if (!strcmp(OutputSelectionType,"MAX_DIST_FROM_ROOT"))
      {
      // Compute the vertex that is furthest from the root (but still
      // reachable).
      vtkIdType maxFromRootVertex = this->OriginVertexIndex;
      double maxDistance = 0.0;
      vtkSmartPointer<vtkVertexListIterator> vertices
        = vtkSmartPointer<vtkVertexListIterator>::New();
      output->GetVertices(vertices);
      while (vertices->HasNext())
        {
        vtkIdType v = vertices->Next();
        double dist = PathLengthArray->GetTuple1(helper->GetVertexIndex(v));
        if (dist != VTK_DOUBLE_MAX && dist > maxDistance)
          {
          maxFromRootVertex = v;
          maxDistance = dist;
          }
        }

      // Combine the furthest-from-root information from all
      // processes.
      using namespace boost::parallel;
      maxFromRootVertex = all_reduce(pbglHelper->GetProcessGroup(),
                                     vtkstd::make_pair(maxFromRootVertex,
                                                       maxDistance),
                                     furthest_vertex_double(output)).first;

      ids->InsertNextValue(maxFromRootVertex);
      }

    node->SetSelectionList(ids);
    node->SetContentType(vtkSelectionNode::INDICES);
    node->SetFieldType(vtkSelectionNode::POINT);
    sel->AddNode(node);
    }

  return 1;
}

void vtkPBGLShortestPaths::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);

  os << indent << "OriginVertexIndex: " << this->OriginVertexIndex << endl;

  os << indent << "InputArrayName: "
     << (this->InputArrayName ? this->InputArrayName : "(none)") << endl;

  os << indent << "EdgeWeightArrayName: "
     << (this->EdgeWeightArrayName ? this->EdgeWeightArrayName : "(none)")
     << endl;

  os << indent << "Delta: " << this->Delta << endl;

  os << indent << "PredecessorArrayName: "
     << (this->PredecessorArrayName ? this->PredecessorArrayName : "(none)")
     << endl;

  os << indent << "PathLengthArrayName: "
     << (this->PathLengthArrayName ? this->PathLengthArrayName : "(none)")
     << endl;

  os << indent << "OriginValue: " << this->OriginValue.ToString() << endl;

  os << indent << "OutputSelection: "
     << (this->OutputSelection ? "on" : "off") << endl;

  os << indent << "OriginFromSelection: "
     << (this->OriginFromSelection ? "on" : "off") << endl;

  os << indent << "UseUniformEdgeWeights: "
     << (this->UseUniformEdgeWeights ? "on" : "off") << endl;

  os << indent << "OutputSelectionType: "
     << (this->OutputSelectionType ? this->OutputSelectionType : "(none)") << endl;
}

//----------------------------------------------------------------------------
int vtkPBGLShortestPaths::FillInputPortInformation(
  int port, vtkInformation* info)
{
  // now add our info
  if (port == 0)
    {
    info->Set(vtkAlgorithm::INPUT_REQUIRED_DATA_TYPE(), "vtkGraph");
    }
  else if (port == 1)
    {
    info->Set(vtkAlgorithm::INPUT_REQUIRED_DATA_TYPE(), "vtkSelection");
    info->Set(vtkAlgorithm::INPUT_IS_OPTIONAL(), 1);
    }
  return 1;
}

//----------------------------------------------------------------------------
int vtkPBGLShortestPaths::FillOutputPortInformation(
  int port, vtkInformation* info)
{
  // now add our info
  if (port == 0)
    {
    info->Set(vtkDataObject::DATA_TYPE_NAME(), "vtkGraph");
    }
  else if (port == 1)
    {
    info->Set(vtkDataObject::DATA_TYPE_NAME(), "vtkSelection");
    }
  return 1;
}


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

// Modifications from original library:
//  * changed inclusion protection tag
//  * moved to namespace CGAL::internal::
//  * add parameter "reset_trees" to train() to be able to construct
//    forest with several iterations

#ifndef CGAL_INTERNAL_LIBLEARNING_RANDOMFOREST_FOREST_H
#define CGAL_INTERNAL_LIBLEARNING_RANDOMFOREST_FOREST_H
#include "common-libraries.hpp"
#include "tree.hpp"
#include <boost/ptr_container/serialize_ptr_vector.hpp>
#if VERBOSE_TREE_PROGRESS
#include <cstdio>
#endif

namespace CGAL { namespace internal {

namespace liblearning {
namespace RandomForest {

template <typename NodeT>
class RandomForest {
public:
    typedef typename NodeT::ParamType ParamType;
    typedef typename NodeT::FeatureType FeatureType;
    typedef Tree<NodeT> TreeType;
    ParamType params;

    std::vector<uint8_t> was_oob_data;
    DataView2D<uint8_t> was_oob;

    boost::ptr_vector< Tree<NodeT> > trees;

    RandomForest() {}
    RandomForest(ParamType const& params) : params(params) {}

    template<typename SplitGenerator>
    void train(DataView2D<FeatureType> samples,
               DataView2D<int> labels,
               DataView2D<int> train_sample_idxes,
               SplitGenerator const& split_generator,
               size_t seed_start = 1,
               bool register_oob = true,
               bool reset_trees = true
               )
    {
        if (reset_trees)
          trees.clear();

        params.n_classes  = *std::max_element(&labels(0,0), &labels(0,0)+labels.num_elements()) + 1;
        params.n_features = samples.cols;
        params.n_samples  = samples.rows;

        std::vector<int> sample_idxes;

        if (train_sample_idxes.empty()) {
            // no indexes were passed, generate vector with all indexes
            sample_idxes.resize(params.n_samples);
            for (size_t i_sample = 0; i_sample < params.n_samples; ++i_sample) {
                sample_idxes[i_sample] = i_sample;
            }
        } else {
            // copy indexes
            sample_idxes.assign(&train_sample_idxes(0,0), &train_sample_idxes(0,0)+train_sample_idxes.num_elements());
        }

        size_t n_idxes = sample_idxes.size();
        params.n_in_bag_samples = n_idxes * (1 - params.sample_reduction);

        // Random distribution over indexes
        UniformIntDist dist(0, n_idxes - 1);

        // Store for each sample and each tree if sample was used for tree
        if (register_oob) {
            was_oob_data.assign(n_idxes*params.n_trees, 1);
            was_oob = DataView2D<uint8_t>(&was_oob_data[0], n_idxes, params.n_trees);
        }

        std::size_t nb_trees = trees.size();
        for (size_t i_tree = nb_trees; i_tree < nb_trees + params.n_trees; ++i_tree) {
#if VERBOSE_TREE_PROGRESS
            std::printf("Training tree %zu/%zu, max depth %zu\n", i_tree+1, nb_trees + params.n_trees, params.max_depth);
#endif
            // new tree
            trees.push_back(new TreeType(&params));
            // initialize random generator with sequential seeds (one for each
            // tree)
            RandomGen gen(seed_start + i_tree);
            // Bagging: draw random sample indexes used for this tree
            std::vector<int> in_bag_samples(params.n_in_bag_samples);
            for (size_t i_sample = 0; i_sample < in_bag_samples.size(); ++i_sample) {
                int random_idx = dist(gen);
                in_bag_samples[i_sample] = sample_idxes[random_idx];
                if (register_oob && was_oob(random_idx, i_tree)) {
                    was_oob(random_idx, i_tree) = 0;
                }
            }
#ifdef TREE_GRAPHVIZ_STREAM
            TREE_GRAPHVIZ_STREAM << "digraph Tree {" << std::endl;
#endif
            // Train the tree
            trees.back().train(samples, labels, &in_bag_samples[0], in_bag_samples.size(), split_generator, gen);
#ifdef TREE_GRAPHVIZ_STREAM
            TREE_GRAPHVIZ_STREAM << "}" << std::endl << std::endl;
#endif
        }
    }
    int evaluate(FeatureType const* sample, float* results) {
        // initialize output probabilities to 0
        std::fill_n(results, params.n_classes, 0.0f);
        // accumulate votes of the trees
        for (size_t i_tree = 0; i_tree < trees.size(); ++i_tree) {
            float const* tree_result = trees[i_tree].evaluate(sample);
            for (size_t i_cls = 0; i_cls < params.n_classes; ++i_cls) {
                results[i_cls] += tree_result[i_cls];
            }
        }
        float best_val   = 0.0;
        int   best_class = 0;
        float scale      = 1.0 / trees.size();
        for (size_t i_cls = 0; i_cls < params.n_classes; ++i_cls) {
            // divide by number of trees to normalize probability
            results[i_cls] *= scale;
            // determine best class
            if (results[i_cls] > best_val) {
                best_val = results[i_cls];
                best_class = i_cls;
            }
        }
        return best_class;
    }
#if 0
    float similarity_endnode(float const* sample_1, float const* sample_2) {
        double sum = 0.0;
        for (size_t i_tree = 0; i_tree < trees.size(); ++i_tree) {
            sum += trees[i_tree].similarity_endnode(sample_1, sample_2);
        }
        return sum/trees.size();
    }
    float similarity_path(float const* sample_1, float const* sample_2) {
        double sum = 0.0;
        for (size_t i_tree = 0; i_tree < trees.size(); ++i_tree) {
            sum += trees[i_tree].similarity_path(sample_1, sample_2);
        }
        return sum/trees.size();
    }
#endif
    template <typename Archive>
    void serialize(Archive& ar, unsigned /* version */)
    {
        ar & BOOST_SERIALIZATION_NVP(params);
        ar & BOOST_SERIALIZATION_NVP(trees);
    }
};

}
}

}} // namespace CGAL::internal::

#endif


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

#include <boost/dispatch/meta/property_of.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace meta
{
  template<class Tag, class T>
  struct constant_value
       : Tag::template apply<typename boost::dispatch::meta::
                             property_of<typename boost::dispatch::meta::
                                         scalar_of<T>::type
                                        >::type
                            >
  {
  };
} } }

#endif