#ifdef _OPENMP
#include <omp.h>
#endif
#include <Python.h>
#include <numpy/ndarrayobject.h>
#include <limits>
#include <iostream>
#include <boost/assert.hpp>
#include <boost/python.hpp>
#include <kgraph.h>
#include <kgraph-data.h>

#ifdef USE_BLAS
extern "C" {
    enum CBLAS_ORDER {CblasRowMajor=101, CblasColMajor=102};
    enum CBLAS_TRANSPOSE {CblasNoTrans=111, CblasTrans=112, CblasConjTrans=113};
    void cblas_sgemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA,
                 const enum CBLAS_TRANSPOSE TransB, const int M, const int N,
                 const int K, const float alpha, const float *A,
                 const int lda, const float *B, const int ldb,
                 const float beta, float *C, const int ldc);
    void cblas_dgemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA,
                 const enum CBLAS_TRANSPOSE TransB, const int M, const int N,
                 const int K, const double alpha, const double *A,
                 const int lda, const double *B, const int ldb,
                 const double beta, double *C, const int ldc);
}

static void blas_prod (kgraph::MatrixProxy<float> const &p1, float const *p2, int n2, int l2, kgraph::Matrix<float> *r) {
    r->zero();
    cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, p1.size(), n2, p1.dim(),
                1.0, p1[0], p1[1]-p1[0], p2, l2, 0, (*r)[0], (*r)[1]-(*r)[0]);
}

static void blas_prod (kgraph::MatrixProxy<double> const &p1, double const *p2, int n2, int l2, kgraph::Matrix<double> *r) {
    r->zero();
    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, p1.size(), n2, p1.dim(),
                1.0, p1[0], p1[1]-p1[0], p2, l2, 0, (*r)[0], (*r)[1]-(*r)[0]);
}

static unsigned constexpr MIN_BLAS_BATCH_SIZE = 16;
#endif

using namespace std;
namespace python = boost::python;

/// Oracle for matrix data.
/** DATA_TYPE can be Matrix or MatrixProxy,
* DIST_TYPE should be one class within the namespace kgraph.metric.
*/
namespace {

    PyObject *load_lshkit (string const &path, PyObject *type) {
        static const unsigned LSHKIT_HEADER = 3;
        std::ifstream is(path.c_str(), std::ios::binary);
        unsigned header[LSHKIT_HEADER]; /* entry size, row, col */
        is.read((char *)header, sizeof header);
        if (!is) throw kgraph::io_error(path);
        unsigned elsize = header[0];
        unsigned N = header[1];
        unsigned D = header[2];
        npy_intp dims[2] = {N, D};
        PyArray_Descr *descr = PyArray_DescrFromTypeObject(type);
        if (!(descr->elsize == elsize)) throw kgraph::io_error(path);
        PyObject *nd = PyArray_SimpleNewFromDescr(2, &dims[0], descr);
        if ((!nd) || (PyArray_ITEMSIZE(nd) != elsize)) throw kgraph::runtime_error("error creating numpy matrix");
        float *buf = reinterpret_cast<float*>(PyArray_DATA(nd));
        is.read((char *)&buf[0], 1LL * N * D * elsize);
        if (!is) throw kgraph::io_error(path);
        return nd;
    }

    struct EuclideanLike {
        template <typename T>
        static float norm (T const *t1, unsigned dim) {
            return kgraph::metric::l2sqr::norm2(t1, dim)/2;
        }
        static float dist (float dot, float n1, float n2) {
            return n1 + n2 - dot;
        }
    };

    struct AngularLike {
        template <typename T>
        static float norm (T const *t1, unsigned dim) {
            return std::sqrt(kgraph::metric::l2sqr::norm2(t1, dim));
        }
        static float dist (float dot, float n1, float n2) {
            return -dot/(n1 * n2 + std::numeric_limits<float>::epsilon());
        }
    };

    template <typename DATA_TYPE, typename METRIC>
    class NDArrayOracle: public kgraph::IndexOracle {
        PyArrayObject *handle; // we need to keep reference
        kgraph::MatrixProxy<DATA_TYPE> proxy;
        vector<float> n2;
    public:
        class SearchOracle: public kgraph::SearchOracle {
            kgraph::MatrixProxy<DATA_TYPE> proxy;
            float const *proxy_n2;
            DATA_TYPE const *query;
            float n2;
        public:
            SearchOracle (kgraph::MatrixProxy<DATA_TYPE> const &p, float const *p_n2, DATA_TYPE const *q)
                : proxy(p), proxy_n2(p_n2), query(q) {
                n2 = METRIC::norm(q, proxy.dim());
            }
            virtual unsigned size () const {
                return proxy.size();
            }
            virtual float operator () (unsigned i) const {
                return METRIC::dist(kgraph::metric::l2sqr::dot(proxy[i], query, proxy.dim()), proxy_n2[i], n2);
            }
        };

        NDArrayOracle (PyArrayObject *data): handle(data), proxy(data), n2(proxy.size()) {
            Py_INCREF(handle);
            for (unsigned i = 0; i < proxy.size(); ++i) {
                n2[i] = METRIC::norm(proxy[i], proxy.dim());
            }
        }

        ~NDArrayOracle () {
            Py_DECREF(handle);
        }

        virtual unsigned size () const {
            return proxy.size();
        }

        virtual float operator () (unsigned i, unsigned j) const {
            return METRIC::dist(kgraph::metric::l2sqr::dot(proxy[i], proxy[j], proxy.dim()), n2[i], n2[j]);
        }

        SearchOracle query (DATA_TYPE const *query) const {
            return SearchOracle(proxy, &n2[0], query);
        }

#ifdef USE_BLAS
        template <typename MATRIX_TYPE>
        void blasQuery (MATRIX_TYPE const &q_proxy,
                unsigned K, float epsilon,
                kgraph::MatrixProxy<unsigned, 1> *ids, kgraph::MatrixProxy<float, 1> *dists) const {
            BOOST_VERIFY(dists);
            static unsigned constexpr BLOCK_SIZE = 1024;
            unsigned block = BLOCK_SIZE;
            if (block < K) {
                block = K;
            }
            if (block >= proxy.size()) {
                block = proxy.size();
            }
            BOOST_VERIFY(block >= K);
            kgraph::Matrix<DATA_TYPE> dot(q_proxy.size(), block);

            vector<float> qn2s(q_proxy.size());
            for (unsigned i = 0; i < q_proxy.size(); ++i) {
                qn2s[i] = METRIC::norm(q_proxy[i], q_proxy.dim());
            }

            unsigned begin = 0; // divide all data into blocks
            while (begin < proxy.size()) {
                unsigned end = begin + block;
                if (end > proxy.size()) {
                    end = proxy.size();
                }
                blas_prod(q_proxy, proxy[begin], end-begin, proxy[1]-proxy[0], &dot);
                // do one block
                if (begin == 0) {
                    // first block
#pragma omp parallel for
                    for (unsigned i = 0; i < q_proxy.size(); ++i) {
                        DATA_TYPE *row = dot[i];
                        DATA_TYPE qn2 = qn2s[i];
                        vector<pair<DATA_TYPE, unsigned>> rank(end-begin);
                        for (unsigned j = 0; j < rank.size(); ++j) {
                            rank[j] = std::make_pair(METRIC::dist(qn2,n2[j],row[j]), j);
                        }
                        std::sort(rank.begin(), rank.end());
                        unsigned *pid = (*ids)[i];
                        float *pdist = (*dists)[i];
                        for (unsigned j = 0; j < K; ++j) {
                            pid[j] = rank[j].second;
                            pdist[j] = rank[j].first;
                        }
                    }
                }
                else { // subsequent blocks, using inserting instead of sort
#pragma omp parallel for
                    for (unsigned i = 0; i < q_proxy.size(); ++i) {
                        DATA_TYPE *row = dot[i];
                        DATA_TYPE qn2 = qn2s[i];
                        unsigned *pid = (*ids)[i];
                        float *pdist = (*dists)[i];
                        for (unsigned j = 0; j < end-begin; ++j) {
                            // insert
                            unsigned id = begin + j;
                            float d = METRIC::dist(qn2, n2[id], row[j]);
                            unsigned c = K-1;
                            if (d >= pdist[c]) continue;
                            while ((c > 0) && (d < pdist[c-1])) {
                                pid[c] = pid[c-1];
                                pdist[c] = pdist[c-1];
                                --c;
                            }
                            pid[c] = id;
                            pdist[c] = d;
                        }
                    }
                }
                begin = end;
            }
        }
#endif
    };

    struct IndexParams: public kgraph::KGraph::IndexParams {
    };

    struct SearchParams: public kgraph::KGraph::SearchParams {
        unsigned threads;
        bool withDistance;
        bool blas;
        SearchParams (): threads(0), withDistance(false), blas(false)  {
        }
    };

    void check_array (PyArrayObject *array, float) {
        do {
            if (array->nd != 2) break;
            if (array->dimensions[1] % 4) break;
            if (array->descr->type_num != NPY_FLOAT) break;
            return;
        } while (false);
        throw kgraph::invalid_argument("bad array type");
    }

    void check_array (PyArrayObject *array, double) {
        do {
            if (array->nd != 2) break;
            if (array->dimensions[1] % 4) break;
            if (array->descr->type_num != NPY_DOUBLE) break;
            return;
        } while (false);
        throw kgraph::invalid_argument("bad array type");
    }

    class ImplBase {
    protected:
        kgraph::KGraph *index;
        bool hasIndex;
    public:
        ImplBase (): index(kgraph::KGraph::create()), hasIndex(false) {
            if (!index) throw runtime_error("error creating kgraph instance");
        }

        virtual ~ImplBase () {
            delete index;
        }

        virtual void build (IndexParams params) = 0;
        virtual python::object search (PyArrayObject *, SearchParams) const = 0;

        void load (char const *path) {
            index->load(path);
            hasIndex = true;
        }
        void save (char const *path) const {
            index->save(path);
        }
    };

    template <typename DATA_TYPE, typename METRIC_TYPE>
    class Impl: public ImplBase {
        NDArrayOracle<DATA_TYPE, METRIC_TYPE> oracle;
    public:
        Impl (PyArrayObject *data): oracle(data) {
            check_array(data, DATA_TYPE());
        }

        void build (IndexParams params) {
            index->build(oracle, params, NULL);
            hasIndex = true;
        }

        python::object search (PyArrayObject *query, SearchParams params) const {
            check_array(query, DATA_TYPE());
            kgraph::MatrixProxy<DATA_TYPE> qmatrix(query);
            npy_intp dims[] = {qmatrix.size(), params.K};
            PyObject *result =  PyArray_SimpleNew(2, dims, NPY_UINT32);
            PyObject *distance =  PyArray_SimpleNew(2, dims, NPY_FLOAT);
            Py_BEGIN_ALLOW_THREADS
            kgraph::MatrixProxy<unsigned, 1> rmatrix(reinterpret_cast<PyArrayObject *>(result));
            kgraph::MatrixProxy<float, 1> distmatrix(reinterpret_cast<PyArrayObject *>(distance));
#ifdef _OPENMP
            if (params.threads) {
                params.threads = ::omp_get_num_threads();
                ::omp_set_num_threads(params.threads);
            }
#endif
#ifdef USE_BLAS
            if (params.blas && (qmatrix.size() >= MIN_BLAS_BATCH_SIZE)) {
                oracle.blasQuery(qmatrix, params.K, params.epsilon, &rmatrix, &distmatrix);
            }
            else
#endif
                if (hasIndex) {
#pragma omp parallel for
                for (unsigned i = 0; i < qmatrix.size(); ++i) {
                    if (params.withDistance) {
                        index->search(oracle.query(qmatrix[i]), params, const_cast<unsigned *>(rmatrix[i]),
                                      const_cast<float *>(distmatrix[i]),NULL);
                    }
                    else {
                        index->search(oracle.query(qmatrix[i]), params, const_cast<unsigned *>(rmatrix[i]), NULL);
                    }
                }
            }
            else {
#pragma omp parallel for
                for (unsigned i = 0; i < qmatrix.size(); ++i) {
                    if (params.withDistance) {
                        oracle.query(qmatrix[i]).search(params.K, params.epsilon, const_cast<unsigned *>(rmatrix[i]),
                                     const_cast<float *>(distmatrix[i]));
                    }
                    else {
                        oracle.query(qmatrix[i]).search(params.K, params.epsilon, const_cast<unsigned *>(rmatrix[i]), NULL);
                    }
                }
            }
            Py_END_ALLOW_THREADS

            if (params.withDistance) {
                PyObject* tup = PyTuple_New(2);
                PyTuple_SetItem(tup,0,result);
                PyTuple_SetItem(tup,1,distance);
                return python::object(python::handle<>(tup));
            }
            else {
                Py_DECREF(distance);
                return python::object(python::handle<>(result));
            }
#ifdef _OPENMP
            if (params.threads) {
                ::omp_set_num_threads(params.threads);
            }
#endif
        }
    };
}

class KGraph {
    ImplBase *impl;
public:
    KGraph () {
        cerr << "!!!!!!!!!!" << endl;
        cerr << "pykgraph API has been changed" << endl;
        cerr << "Old:" << endl;
        cerr << "   index = pykgraph.KGraph()" << endl;
        cerr << "   index.build(dataset, ...)" << endl;
        cerr << "   index.search(dataset, query, ...)" << endl;
        cerr << "New (dataset passed in constructor):" << endl;
        cerr << "   index = pykgraph.KGraph(dataset, metric)" << endl;
        cerr << "   # metric: 'euclidean' or 'angular'" << endl;
        cerr << "   index.build(...)" << endl;
        cerr << "   index.search(query, ...)" << endl;
        cerr << "!!!!!!!!!!" << endl;
        throw kgraph::invalid_argument("obsolete constructor");
    }

    KGraph (PyObject *data, string const &metric): impl(nullptr) {
        PyArrayObject *pd = reinterpret_cast<PyArrayObject *>(data);
        if (!pd) throw kgraph::invalid_argument("bad array");
        if (metric == "euclidean") {
            switch (pd->descr->type_num) {
                case NPY_FLOAT: impl = new Impl<float, EuclideanLike>(pd); break;
                case NPY_DOUBLE: impl = new Impl<double, EuclideanLike>(pd); break;
            }
        }
        else if (metric == "angular") {
            switch (pd->descr->type_num) {
                case NPY_FLOAT: impl = new Impl<float, AngularLike>(pd); break;
                case NPY_DOUBLE: impl = new Impl<double, AngularLike>(pd); break;
            }
        }
        else throw runtime_error("metric not supported");
        if (!impl) throw runtime_error("data type not supported.");
    }

    ~KGraph () {
        delete impl;
    }

    void load (char const *path) {
        impl->load(path);
    }

    void save (char const *path) const {
        impl->save(path);
    }

    void build (unsigned iterations,
               unsigned L,
               unsigned K,
               unsigned S,
               unsigned controls,
               float delta,
               float recall,
               unsigned prune,
               int reverse) {
        IndexParams params;
        params.iterations = iterations;
        params.L = L;
        params.K = K;
        params.S = S;
        params.controls = controls;
        params.delta = delta;
        params.recall = recall;
        params.prune = prune;
        params.reverse = reverse;
        impl->build(params);
        /*
        PyArrayObject *pd = reinterpret_cast<PyArrayObject *>(data.ptr());
        switch (pd->descr->type_num) {
            case NPY_FLOAT: buildImpl<float>(params); return;
            case NPY_DOUBLE: buildImpl<double>(params); return;
        }
        throw runtime_error("data type not supported.");
        */
    }
    python::object search (PyObject *query,
                unsigned K,
                unsigned P,
                unsigned M,
                unsigned S,
                unsigned T,
                unsigned threads,
                bool withDistance,
                bool blas) {
        SearchParams params;
        params.K = K;
        params.P = P;
        params.M = M;
        params.S = S;
        params.T = T;
        params.threads = threads;
        params.withDistance = withDistance;
        params.blas = blas;
        return impl->search(reinterpret_cast<PyArrayObject *>(query), params);
    }
};

BOOST_PYTHON_MODULE(pykgraph)
{
    import_array();
    python::numeric::array::set_module_and_type("numpy", "ndarray");
    python::class_<KGraph>("KGraph", python::init<PyObject *, string>())
        .def(python::init<>())
        //.def(python::init<PyObject *, string>())
            //  (python::arg("data"), python::arg("metric") = "euclidean")))
        .def("load", &KGraph::load)
        .def("save", &KGraph::save)
        .def("build", &KGraph::build,
             (python::arg("data"),
              python::arg("iterations") = kgraph::default_iterations,
              python::arg("L") = kgraph::default_L,
              python::arg("K") = kgraph::default_K,
              python::arg("S") = kgraph::default_S,
              python::arg("controls") = kgraph::default_controls,
              python::arg("delta") = kgraph::default_delta,
              python::arg("recall") = kgraph::default_recall,
              python::arg("prune") = kgraph::default_prune,
              python::arg("reverse") = kgraph::default_reverse))
        .def("search", &KGraph::search,
            (python::arg("data"),
             python::arg("query"),
             python::arg("K") = kgraph::default_K,
             python::arg("P") = kgraph::default_P,
             python::arg("M") = kgraph::default_M,
             python::arg("S") = kgraph::default_S,
             python::arg("T") = kgraph::default_T,
             python::arg("threads") = 0,
             python::arg("withDistance") = false,
             python::arg("blas") = false))
        ;
    python::def("load_lshkit", ::load_lshkit);
    python::def("version", kgraph::KGraph::version);
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file multi_sensor_sigma_point_update_policy.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/util/types.hpp>
#include <fl/util/traits.hpp>
#include <fl/util/descriptor.hpp>
#include <fl/model/sensor/joint_sensor_iid.hpp>
#include <fl/filter/gaussian/transform/point_set.hpp>
#include <fl/filter/gaussian/quadrature/sigma_point_quadrature.hpp>

namespace fl
{

// Forward declarations
template <typename...> class MultiSensorSigmaPointUpdatePolicy;

/**
 * \internal
 */
template <
    typename SigmaPointQuadrature,
    typename NonJoinSensor
>
class MultiSensorSigmaPointUpdatePolicy<
          SigmaPointQuadrature,
          NonJoinSensor>
{
    static_assert(
        std::is_base_of<
            internal::JointSensorIidType, NonJoinSensor
        >::value,
        "\n\n\n"
        "====================================================================\n"
        "= Static Assert: You are using the wrong observation model type    =\n"
        "====================================================================\n"
        "  Observation model type must be a JointSensor<...>.      \n"
        "  For single observation model, use the regular Gaussian filter     \n"
        "  or the regular SigmaPointUpdatePolicy if you are specifying       \n"
        "  the update policy explicitly fo the GaussianFilter.               \n"
        "====================================================================\n"
    );
};

/**
 * \brief Represents an update policy update for multiple sensors. This instance
 *        expects a \a JointSensor<>. The model is forwarded  as
 *        NonAdditive<JointSensor> to the actual
 *        implementation. In case you want to use the model as Additive, you may
 *        specify this explicitly using Additive<JointSensor> or
 *        UseAsAdditive<JointSensor>::Type
 */
template <
    typename SigmaPointQuadrature,
    typename MultipleOfLocalSensor
>
class MultiSensorSigmaPointUpdatePolicy<
          SigmaPointQuadrature,
          JointSensor<MultipleOfLocalSensor>>
    : public MultiSensorSigmaPointUpdatePolicy<
                SigmaPointQuadrature,
                NonAdditive<JointSensor<MultipleOfLocalSensor>>>
{ };

/**
 * \brief Represents an update policy update functor for multiple sensors.
 *        This instance expects a \a NonAdditive<JointSensor<>>.
 *        The implementation exploits factorization in the joint observation.
 *        The update is performed for each sensor separately.
 */
template <
    typename SigmaPointQuadrature,
    typename MultipleOfLocalSensor
>
class MultiSensorSigmaPointUpdatePolicy<
          SigmaPointQuadrature,
          NonAdditive<JointSensor<MultipleOfLocalSensor>>>
    : public Descriptor
{
public:
    typedef JointSensor<MultipleOfLocalSensor> JointModel;

    typedef typename JointModel::State State;
    typedef typename JointModel::Obsrv Obsrv;
    typedef typename JointModel::LocalObsrv LocalObsrv;
    typedef typename JointModel::LocalNoise LocalObsrvNoise;

    template <typename Belief>
    void operator()(JointModel& obsrv_function,
                    const SigmaPointQuadrature& quadrature,
                    const Belief& prior_belief,
                    const Obsrv& y,
                    Belief& posterior_belief)
    {
        auto& sensor_model = obsrv_function.local_sensor();

        /* ------------------------------------------ */
        /* - Determine the number of quadrature     - */
        /* - points needed for the given quadrature - */
        /* - in conjunction with the joint Gaussian - */
        /* - p(State, LocalObsrvNoise)              - */
        /* ------------------------------------------ */
        enum : signed int
        {
            NumberOfPoints = SigmaPointQuadrature::number_of_points(
                                 JoinSizes<
                                     SizeOf<State>::Value,
                                     SizeOf<LocalObsrvNoise>::Value
                                 >::Size)
        };

        /* ------------------------------------------ */
        /* - PointSets                              - */
        /* - [p_X, p_Q] ~ p(State, LocalObsrvNoise) - */
        /* ------------------------------------------ */
        PointSet<State, NumberOfPoints> p_X;
        PointSet<LocalObsrvNoise, NumberOfPoints> p_Q;

        /* ------------------------------------------ */
        /* - PointSet [p_Y] = h(p_X, p_Q)           - */
        /* ------------------------------------------ */
        PointSet<LocalObsrv, NumberOfPoints> p_Y;

        /* ------------------------------------------ */
        /* - Transform p(State, LocalObsrvNoise) to - */
        /* - point sets [p_X, p_Q]                  - */
        /* ------------------------------------------ */
        quadrature.transform_to_points(
            prior_belief,
            Gaussian<LocalObsrvNoise>(sensor_model.noise_dimension()),
            p_X,
            p_Q);

        /* ------------------------------------------ */
        /* - Compute expected moments of the state  - */
        /* - E[X], Cov(X, X)                        - */
        /* ------------------------------------------ */
        auto W = p_X.covariance_weights_vector().asDiagonal();
        auto mu_x = p_X.mean();
        auto X = p_X.centered_points();
        auto c_xx_inv = (X * W * X.transpose()).inverse().eval();

        /* ------------------------------------------ */
        /* - Temporary accumulators which will be   - */
        /* - used to updated the belief             - */
        /* ------------------------------------------ */
        auto C = c_xx_inv;
        auto D = State();
        D.setZero(mu_x.size());

        const int sensor_count = obsrv_function.count_local_models();
        const int dim_y = y.size() / sensor_count;

        /* ------------------------------------------ */
        /* - lambda of the sensor observation       - */
        /* - function                               - */
        /* ------------------------------------------ */
        auto&& h = [&](const State& x, const LocalObsrvNoise& w)
        {
            return sensor_model.observation(x, w);
        };

        for (int i = 0; i < sensor_count; ++i)
        {
            // validate sensor value, i.e. make sure it is finite
            if (!is_valid(y, i * dim_y, i * dim_y + dim_y)) continue;

            // select current sensor and propagate the points through h(x, w)
            sensor_model.id(i);
            quadrature.propagate_points(h, p_X, p_Q, p_Y);

            // comute expected moments of the observation and validate
            auto mu_y = p_Y.mean();
            if (!is_valid(mu_y, 0, dim_y)) continue;

            // update accumulatorsa according to the equations in PAPER REF
            auto Y = p_Y.centered_points();
            auto c_xy = (X * W * Y.transpose()).eval();
            auto c_yx = c_xy.transpose().eval();
            auto A_i = (c_yx * c_xx_inv).eval();
            auto c_yy_given_x = (
                     (Y * W * Y.transpose()) - c_yx * c_xx_inv * c_xy
                 ).eval();

            auto innovation = (y.middleRows(i * dim_y, dim_y) - mu_y).eval();
            C += A_i.transpose() * solve(c_yy_given_x, A_i);
            D += A_i.transpose() * solve(c_yy_given_x, innovation);
        }

        /* ------------------------------------------ */
        /* - Update belief according to PAPER REF   - */
        /* ------------------------------------------ */
        // make sure the posterior has the correct dimension
        posterior_belief.dimension(prior_belief.dimension());
        posterior_belief.covariance(C.inverse());
        posterior_belief.mean(mu_x + posterior_belief.covariance() * D);
    }

    virtual std::string name() const
    {
        return "MultiSensorSigmaPointUpdatePolicy<"
                + this->list_arguments(
                       "SigmaPointQuadrature",
                       "NonAdditive<SensorFunction>")
                + ">";
    }

    virtual std::string description() const
    {
        return "Multi-Sensor Sigma Point based filter update policy "
               "for joint observation model of multiple local observation "
               "models with non-additive noise.";
    }

private:
    /**
     * \brief Checks whether all vector components within the range (start, end)
     *        are finiate, i.e. not NAN nor Inf.
     */
    template <typename Vector>
    bool is_valid(Vector&& vector, int start, int end) const
    {
        for (int k = start; k < end; ++k)
        {
            if (!std::isfinite(vector(k))) return false;
        }

        return true;
    }
};

}


#include <xfel/mono_simulation/bandpass_gaussian.h>
#include <boost/math/special_functions/erf.hpp>
double
xfel::parameter::best_fit_limit( farray const& W, farray const& WS ){
  double wsum = 0.0;
  for (int i=0; i<W.size(); ++i){
    wsum += W[i];
  }
  return wsum/W.size();
}


// readfacevmd - reads facial expression from photo / movie and generate a VMD motion file.

// OpenFace Headers
#include <LandmarkCoreIncludes.h>
#include <FaceAnalyser.h>
#include <SequenceCapture.h>
#include <GazeEstimation.h>

#include <opencv2/core/core.hpp>
#include <boost/filesystem.hpp>
#include <dlib/image_processing/frontal_face_detector.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include "smooth_reduce.h"
#include "MMDFileIOUtil.h"
#include "VMD.h"
#include "morph_name.h"
#include "refine.h"

#define _USE_MATH_DEFINES
#include <math.h>

#define RFV_DLL_EXPORT
#include "readfacevmd.h"

using namespace std;
using namespace Eigen;

const int LANDMARK_NUM = 68;

// Action Unit ID
enum AUID {
  InnerBrowRaiser = 1,      // AU01 眉の内側を上げる
  OuterBrowRaiser = 2,      // AU02 眉の外側を上げる
  BrowLowerer = 4,          // AU04 眉を下げる
  UpperLidRaiser = 5,       // AU05 目を見開く
  CheekRaiser = 6,          // AU06 頬を上げる
  LidTightener = 7,         // AU07 細目
  NoseWrinkler = 9,         // AU09 鼻に皴を寄せる。怒り
  UpperLipRaiser = 10,      // AU10 上唇を上げる
  LipCornerPuller = 12,     // AU12 口の端を上げる。にやり
  Dimpler = 14,             // AU14 えくぼ
  LipCornerDepressor = 15,  // AU15 への字口
  ChinRaiser = 17,          // AU17 顎を上げる
  LipStrecher = 20,         // AU20 口を横に伸ばす
  LipTightener = 23,        // AU23 口をすぼめる
  LipPart = 25,             // AU25 口を開ける。「い」の口でもtrueになる
  JawDrop = 26,             // AU26 顎を下げる。「あ」の口の判定にはこちらを使う
  LipSuck = 28,             // AU28 唇を吸う
  Blink = 45,               // AU45 まばたき
};
const int AU_SIZE = 46;
const double ACTION_UNIT_MAXVAL = 5.0;

void dumprot(const Quaterniond& rot, string name)
{
  Vector3d v = rot.toRotationMatrix().eulerAngles(0, 1, 2);
  cout << name << ": " << v.x() * 180 / M_PI << "," << v.y()  * 180 / M_PI << "," << v.z()  * 180 / M_PI << endl;
}

// 回転のキーフレームを VMD_Frame の vector に追加する
void add_rotation_pose(vector<VMD_Frame>& frame_vec, const Quaterniond& rot, uint32_t frame_number, string bone_name)
{
    VMD_Frame frame;
    MMDFileIOUtil::utf8_to_sjis(bone_name, frame.bonename, frame.bonename_len);
    frame.number = frame_number;
    frame.rotation.w() = rot.w();
    frame.rotation.x() = rot.x();
    frame.rotation.y() = rot.y();
    frame.rotation.z() = rot.z();
    frame_vec.push_back(frame);
}

// 移動のキーフレームを VMD_Frame の vector に追加する
void add_position_pose(vector<VMD_Frame>& frame_vec, const Vector3f& pos, uint32_t frame_number, string bone_name)
{
    VMD_Frame frame;
    MMDFileIOUtil::utf8_to_sjis(bone_name, frame.bonename, frame.bonename_len);
    frame.number = frame_number;
    frame.position.x() = pos.x();
    frame.position.y() = pos.y();
    frame.position.z() = pos.z();
    frame_vec.push_back(frame);
}

// 頭の向き(回転)のキーフレームを VMD_Frame の vector に格納する
void add_head_pose(vector<VMD_Frame>& frame_vec, const Quaterniond& rot, uint32_t frame_number)
{
  string bone_name = u8"頭";
  add_rotation_pose(frame_vec, rot, frame_number, bone_name);
}

// センターの位置のキーフレームを VMD_Frame の vector に格納する
void add_center_frame(vector<VMD_Frame>& frame_vec, const Vector3f& pos, uint32_t frame_number)
{
  string bone_name = u8"センター";
  add_position_pose(frame_vec, pos, frame_number, bone_name);
}

// 目の向き(回転)のキーフレームを VMD_Frame の vector に追加する
void add_gaze_pose(vector<VMD_Frame>& frame_vec, cv::Point3f gazedir_left, cv::Point3f gazedir_right,
		   const Quaterniond& head_rot, uint32_t frame_number)
{
  Vector3d front = head_rot * Vector3d(0, 0, -1);
  Vector3d leftdir;
  leftdir.x() = gazedir_left.x;
  leftdir.y() = - gazedir_left.y;
  leftdir.z() = gazedir_left.z;
  Quaterniond rot_left = Quaterniond::FromTwoVectors(front, leftdir);
  Vector3d rightdir;
  rightdir.x() = gazedir_right.x;
  rightdir.y() = - gazedir_right.y;
  rightdir.z() = gazedir_right.z;
  Quaterniond rot_right = Quaterniond::FromTwoVectors(front, rightdir);

  // 目の回転量を補正
  // TODO: 補正係数の適切な値を決める
  const double amp_both = 1.0;
  const double amp_each = 0.25;
  rot_right = Quaterniond::Identity().slerp(amp_each, rot_right);
  rot_left = Quaterniond::Identity().slerp(amp_each, rot_left);

  add_rotation_pose(frame_vec, rot_right, frame_number, u8"左目");
  add_rotation_pose(frame_vec, rot_left, frame_number, u8"右目");
}

// 表情フレームを VMD_Morph の vector に追加する
void add_morph_frame(vector<VMD_Morph>& morph_vec, string name, uint32_t frame_number, float weight)
{
  if (weight > 1.0) {
    weight = 1.0;
  }
  if (weight < 0.0) {
    weight = 0.0;
  }
  VMD_Morph morph;
  MMDFileIOUtil::utf8_to_sjis(name, morph.name, morph.name_len);
  morph.frame = frame_number;
  morph.weight = weight;
  morph_vec.push_back(morph);
}

// 顔の動きを表すAction Unitをface_analyserから取り出す
void get_action_unit(double* au, FaceAnalysis::FaceAnalyser face_analyser) {
  for (int i = 0; i < AU_SIZE; i++) {
    au[i] = 0;
  }

  auto intensity = face_analyser.GetCurrentAUsReg();
  auto presence = face_analyser.GetCurrentAUsClass();

  bool valid[AU_SIZE];

  for (auto& p : presence) {
    string id_str = p.first.substr(2, 2); // 数字部分を取り出す
    int id = atoi(id_str.c_str());
    if (p.second == 0) {
      valid[id] = false;
    } else {
      valid[id] = true;
    }
  }

  for (auto& p : intensity) {
    string id_str = p.first.substr(2, 2); // 数字部分を取り出す
    int id = atoi(id_str.c_str());
    if (valid[id]) {
      au[id] = p.second / ACTION_UNIT_MAXVAL;
      // cout << "AU" << id << ": " << au[id] << endl;
    }
  }
}

// 顔の表情を推定して morph_vec に追加する
void estimate_facial_expression(vector<VMD_Morph>& morph_vec, double* au, uint32_t frame_number)
{
  // 口
  double mouth_a = au[AUID::JawDrop] * 2;
  double mouth_i = 0;
  double mouth_u = au[AUID::LipTightener] * 2;
  if (mouth_a < 0.1 && mouth_u < 0.1) {
    mouth_i = au[AUID::LipPart] * 2;
  }
  double mouth_smile = au[AUID::LipCornerPuller];

  add_morph_frame(morph_vec, u8"あ", frame_number, mouth_a);
  add_morph_frame(morph_vec, u8"い", frame_number, mouth_i);
  add_morph_frame(morph_vec, u8"う", frame_number, mouth_u);
  add_morph_frame(morph_vec, u8"にやり", frame_number, mouth_smile);
  add_morph_frame(morph_vec, u8"∧", frame_number, au[AUID::LipCornerDepressor]);

  // 目
  double blink = au[AUID::LidTightener];
  if (au[AUID::Blink] > 0.2) {
    blink = 1.0;
  }
  add_morph_frame(morph_vec, u8"まばたき", frame_number, blink);
  // まばたき/笑いの切り替えは後処理で行う
  add_morph_frame(morph_vec, u8"CheekRaiser", frame_number, au[AUID::CheekRaiser]);

  add_morph_frame(morph_vec, u8"びっくり", frame_number, au[AUID::UpperLidRaiser]);

  // 眉
  add_morph_frame(morph_vec, u8"困る", frame_number, au[AUID::InnerBrowRaiser]);
  // 困る/にこりの切り替えは後処理で行う
  add_morph_frame(morph_vec, u8"真面目", frame_number, au[AUID::OuterBrowRaiser]);
  add_morph_frame(morph_vec, u8"怒り", frame_number, au[AUID::NoseWrinkler]);
  add_morph_frame(morph_vec, u8"下", frame_number, au[AUID::BrowLowerer]);
  add_morph_frame(morph_vec, u8"上", frame_number, au[AUID::UpperLidRaiser]);
}

void init_vmd_header(VMD_Header& h)
{
  memset(h.version, 0, h.version_len);
  strcpy(h.version, "Vocaloid Motion Data 0002");
  memset(h.modelname, 0, h.modelname_len);
  strcpy(h.modelname, "dummy model");
}

// Custom gaze esitmater
// following functions are based on: https://github.com/TadasBaltrusaitis/OpenFace/
cv::Matx33f Euler2RotationMatrix(const cv::Vec3f& eulerAngles)
{
    cv::Matx33f rotation_matrix;

    float s1 = sin(eulerAngles[0]);
    float s2 = sin(eulerAngles[1]);
    float s3 = sin(eulerAngles[2]);

    float c1 = cos(eulerAngles[0]);
    float c2 = cos(eulerAngles[1]);
    float c3 = cos(eulerAngles[2]);

    rotation_matrix(0, 0) = c2 * c3;
    rotation_matrix(0, 1) = -c2 * s3;
    rotation_matrix(0, 2) = s2;
    rotation_matrix(1, 0) = c1 * s3 + c3 * s1 * s2;
    rotation_matrix(1, 1) = c1 * c3 - s1 * s2 * s3;
    rotation_matrix(1, 2) = -c2 * s1;
    rotation_matrix(2, 0) = s1 * s3 - c1 * c3 * s2;
    rotation_matrix(2, 1) = c3 * s1 + c1 * s2 * s3;
    rotation_matrix(2, 2) = c1 * c2;

    return rotation_matrix;
}

cv::Point3f GetPupilPosition(cv::Mat_<float> eyeLdmks3d)
{
    eyeLdmks3d = eyeLdmks3d.t();
    cv::Mat_<float> irisLdmks3d = eyeLdmks3d.rowRange(0, 8);
    cv::Point3f p(mean(irisLdmks3d.col(0))[0], mean(irisLdmks3d.col(1))[0], mean(irisLdmks3d.col(2))[0]);
    return p;
}

void CustomEstimateGaze(const LandmarkDetector::CLNF& clnf_model, cv::Point3f& gaze_absolute, float fx, float fy, float cx, float cy, bool left_eye)
{
    cv::Vec6f headPose = LandmarkDetector::GetPose(clnf_model, fx, fy, cx, cy);
    cv::Vec3f eulerAngles(headPose(3), headPose(4), headPose(5));
    cv::Matx33f rotMat = Euler2RotationMatrix(eulerAngles);

    int part = -1;
    for (size_t i = 0; i < clnf_model.hierarchical_models.size(); ++i)
    {
        if (left_eye && clnf_model.hierarchical_model_names[i].compare("left_eye_28") == 0)
        {
            part = i;
        }
        if (!left_eye && clnf_model.hierarchical_model_names[i].compare("right_eye_28") == 0)
        {
            part = i;
        }
    }

    if (part == -1)
    {
        std::cout << "Couldn't find the eye model, something wrong" << std::endl;
        gaze_absolute = cv::Point3f(0, 0, 0);
        return;
    }

    cv::Mat eyeLdmks3d = clnf_model.hierarchical_models[part].GetShape(fx, fy, cx, cy);

    cv::Point3f pupil = GetPupilPosition(eyeLdmks3d);
    cv::Point3f rayDir = pupil / norm(pupil);

    cv::Mat faceLdmks3d = clnf_model.GetShape(fx, fy, cx, cy);
    faceLdmks3d = faceLdmks3d.t();

    int eyeIdx = 1;
    if (left_eye)
    {
        eyeIdx = 0;
    }

    cv::Mat offsetMat = (cv::Mat_<float>(3, 1) << 0, -3.5, 7.0);
    cv::Mat offsetMatT = (cv::Mat(rotMat) * offsetMat).t();
    cv::Point3f eyeOffset = cv::Point3f(offsetMatT);

    cv::Mat mat36 = faceLdmks3d.row(36 + eyeIdx * 6);
    cv::Mat mat39 = faceLdmks3d.row(39 + eyeIdx * 6);
    cv::Point3f eyelidL = cv::Point3f(mat36);
    cv::Point3f eyelidR = cv::Point3f(mat39);
    cv::Point3f eyeCentre = (eyelidL + eyelidR) / 2.0f;
    cv::Point3f eyeballCentre = eyeCentre + eyeOffset;

    // 2Dに再投影
    float d = eyeCentre.z;
    float l2dx = eyelidL.x * d / eyelidL.z;
    float l2dy = eyelidL.y * d / eyelidL.z;
    float r2dx = eyelidR.x * d / eyelidR.z;
    float r2dy = eyelidR.y * d / eyelidR.z;
    float p2dx = pupil.x * d / pupil.z;
    float p2dy = pupil.y * d / pupil.z;
    float t = (p2dx - r2dx) / (l2dx - r2dx);
    if (t < 0.0) t = 0.0; else if (t > 1.0) t = 1.0;
    float newZ = eyelidR.z + (eyelidL.z - eyelidR.z) * t;
    // 新しいzで、黒目の中心位置を再計算する。
    pupil.x = pupil.x * newZ / pupil.z;
    pupil.y = pupil.y * newZ / pupil.z;
    pupil.z = newZ;
    rayDir = pupil / norm(pupil);

    //cv::Point3f gazeVecAxis = RaySphereIntersect(cv::Point3f(0, 0, 0), rayDir, eyeballCentre, 12) - eyeballCentre;
    cv::Point3f gazeVecAxis = pupil - eyeballCentre;

    gaze_absolute = gazeVecAxis / norm(gazeVecAxis);
}
// Custom gaze esitmater

// image_file_name で指定された画像/動画ファイルから表情を推定して vmd_file_name に出力する
RFV_DLL_DECL int read_face_vmd(const std::string& image_file_name, const std::string& vmd_file_name,
			       float cutoff_freq, float threshold_pos, float threshold_rot, float threshold_morph,
			       const std::string& nameconf_file_name)
{
  map<string, string> rename_map;
  if (nameconf_file_name.length() != 0) {
    rename_map = make_rename_map(string(nameconf_file_name));
  }

  vector<string> arg_str;
  arg_str.push_back("-f");
  arg_str.push_back(image_file_name);
  Utilities::SequenceCapture cap;
  if (! cap.Open(arg_str)) {
    cerr << "Open error" << endl;
    return 1;
  }

  LandmarkDetector::FaceModelParameters model_parameters;
  LandmarkDetector::CLNF face_model(model_parameters.model_location);

  // for Action Unit
  FaceAnalysis::FaceAnalyserParameters face_analysis_params;
  face_analysis_params.OptimizeForImages();
  FaceAnalysis::FaceAnalyser face_analyser(face_analysis_params);

  VMD vmd;
  init_vmd_header(vmd.header);

  float srcfps = cap.fps;
  float tgtfps = 30.0;

  for (uint32_t frame_number = 0; true; frame_number++) {
    cout << "frame:" << frame_number << endl;
    cv::Mat image = cap.GetNextFrame();
    if (image.empty()) {
      break;
    }
    cv::Mat_<uchar> grayscale_image = cap.GetGrayFrame();

    if (! LandmarkDetector::DetectLandmarksInVideo(image, face_model, model_parameters, grayscale_image)) {
      continue;
    }

    // 頭の向きを推定する
    cv::Vec6d head_pose = LandmarkDetector::GetPose(face_model, cap.fx, cap.fy, cap.cx, cap.cy);
    Quaterniond rot_vmd = AngleAxisd(- head_pose[3], Vector3d::UnitX())
      * AngleAxisd(head_pose[4], Vector3d::UnitY())
      * AngleAxisd(- head_pose[5], Vector3d::UnitZ());
    add_head_pose(vmd.frame, rot_vmd, frame_number);
    Vector3f center_pos(head_pose[0], - head_pose[1], (head_pose[2] - 1000));
    center_pos = center_pos * 12.5 / 1000 / 2; // 1m = 12.5ミクセル
    add_center_frame(vmd.frame, center_pos, frame_number);

    // 表情を推定する
    face_analyser.PredictStaticAUsAndComputeFeatures(image, face_model.detected_landmarks);
    double action_unit[AU_SIZE];
    get_action_unit(action_unit, face_analyser);
    estimate_facial_expression(vmd.morph, action_unit, frame_number);

    // 目の向きを推定する
    if (face_model.eye_model) {
      cv::Point3f gazedir_left(0, 0, -1);
      cv::Point3f gazedir_right(0, 0, -1);
      CustomEstimateGaze(face_model, gazedir_left, cap.fx, cap.fy, cap.cx, cap.cy, true);
      CustomEstimateGaze(face_model, gazedir_right, cap.fx, cap.fy, cap.cx, cap.cy, false);

      add_gaze_pose(vmd.frame, gazedir_left, gazedir_right, rot_vmd, frame_number);
    }
  }

  cout << "smoothing & reduction start" << endl;
  cout << "cutoff frequency: " << cutoff_freq << endl;
  cout << "position threshold: " << threshold_pos << endl;
  cout << "rotation threshold: " << threshold_rot << endl;
  cout << "morph threshold: " << threshold_morph << endl;
  smooth_and_reduce(vmd, cutoff_freq, threshold_pos, threshold_rot, threshold_morph,
                    srcfps, tgtfps, false);
  cout << "smoothing & reduction end" << endl;

  refine_morph(vmd);

  cout << "rename morph & bone" << endl;
  rename_morph(vmd, rename_map);
  rename_frame(vmd, rename_map);

  cout << "VMD output start" << endl;
  cout << "output filename: " << vmd_file_name << endl;
  // VMDファイルを書き出す
  ofstream out(vmd_file_name, ios::binary);
  vmd.output(out);
  out.close();
  cout << "VMD output end" << endl;
  return 0;
}


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

#ifndef BOOST_HASH_BLOCK_CYPHERS_MD4_HPP
#define BOOST_HASH_BLOCK_CYPHERS_MD4_HPP

#include <boost/hash/block_cyphers/detail/md4_policy.hpp>

#ifdef BOOST_HASH_SHOW_PROGRESS
#include <cstdio>
#endif

//
// Encrypt implemented directly from the RFC as found at
// http://www.faqs.org/rfcs/rfc1320.html
//
// Decrypt is a straight-forward inverse
//
// In MD4 terminology:
// - plaintext = AA, BB, CC, and DD
// - cyphertext = A, B, C, and D
// - key = M^(i) and X
//

namespace boost {
namespace hashes {
namespace block_cyphers {

class md4 {
  public:
    typedef detail::md4_policy policy_type;

    static unsigned const word_bits = policy_type::word_bits;
    typedef policy_type::word_type word_type;

    static unsigned const key_bits = policy_type::key_bits;
    static unsigned const key_words = policy_type::key_words;
    typedef policy_type::key_type key_type;

    static unsigned const block_bits = policy_type::block_bits;
    static unsigned const block_words = policy_type::block_words;
    typedef policy_type::block_type block_type;

  public:
    md4(key_type const &k) : key(k) {
#ifdef BOOST_HASH_SHOW_PROGRESS
        for (unsigned t = 0; t < key_words; ++t) {
            std::printf("X[%2d] = %.8x\n",
                        t, key[t]);
        }
#endif
    }
  private:
    key_type const key;

  public:
    block_type
    encypher(block_type const &plaintext) {
        return encypher_block(plaintext);
    }
  private:
    block_type
    encypher_block(block_type const &plaintext) {
        return encypher_block(key, plaintext);
    }
    static block_type
    encypher_block(key_type const &key,
                   block_type const &plaintext) {

#ifdef BOOST_HASH_SHOW_PROGRESS
        for (unsigned t = 0; t < block_words; ++t) {
            std::printf("%c%c = %.8x\n",
                        'A'+t, 'A'+t, plaintext[t]);
        }
#endif

        // Initialize working variables with block
        word_type a = plaintext[0], b = plaintext[1],
                  c = plaintext[2], d = plaintext[3];

        // Encypher block
#define BOOST_HASH_MD4_ENCYPHER_STEP(aa, bb, cc, dd, fun, k, s, val) \
            { \
                word_type T = aa \
                            + policy_type::fun(bb,cc,dd) \
                            + key[policy_type::key_index(k)] \
                            + val; \
                aa = policy_type::ROTL<s>(T); \
            }
        for (unsigned t =  0; t < 16; t += 4) {
            BOOST_HASH_MD4_ENCYPHER_STEP(a, b, c, d, ff, t+0,  3, 0x00000000)
            BOOST_HASH_MD4_ENCYPHER_STEP(d, a, b, c, ff, t+1,  7, 0x00000000)
            BOOST_HASH_MD4_ENCYPHER_STEP(c, d, a, b, ff, t+2, 11, 0x00000000)
            BOOST_HASH_MD4_ENCYPHER_STEP(b, c, d, a, ff, t+3, 19, 0x00000000)

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf("Round 1: %.8x %.8x %.8x %.8x\n",
                   a, b, c, d);
#endif
        }
        for (unsigned t = 16; t < 32; t += 4) {
            BOOST_HASH_MD4_ENCYPHER_STEP(a, b, c, d, gg, t+0,  3, 0x5a827999)
            BOOST_HASH_MD4_ENCYPHER_STEP(d, a, b, c, gg, t+1,  5, 0x5a827999)
            BOOST_HASH_MD4_ENCYPHER_STEP(c, d, a, b, gg, t+2,  9, 0x5a827999)
            BOOST_HASH_MD4_ENCYPHER_STEP(b, c, d, a, gg, t+3, 13, 0x5a827999)

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf("Round 2: %.8x %.8x %.8x %.8x\n",
                   a, b, c, d);
#endif
        }
        for (unsigned t = 32; t < 48; t += 4) {
            BOOST_HASH_MD4_ENCYPHER_STEP(a, b, c, d, hh, t+0,  3, 0x6ed9eba1)
            BOOST_HASH_MD4_ENCYPHER_STEP(d, a, b, c, hh, t+1,  9, 0x6ed9eba1)
            BOOST_HASH_MD4_ENCYPHER_STEP(c, d, a, b, hh, t+2, 11, 0x6ed9eba1)
            BOOST_HASH_MD4_ENCYPHER_STEP(b, c, d, a, hh, t+3, 15, 0x6ed9eba1)

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf("Round 3: %.8x %.8x %.8x %.8x\n",
                   a, b, c, d);
#endif
        }

        block_type cyphertext = {{a, b, c, d}};
        return cyphertext;
    }

  public:
    block_type
    decypher(block_type const &plaintext) {
        return decypher_block(plaintext);
    }
  private:
    block_type
    decypher_block(block_type const &plaintext) {
        return decypher_block(key, plaintext);
    }
    static block_type
    decypher_block(key_type const &key,
                   block_type const &cyphertext) {

#ifdef BOOST_HASH_SHOW_PROGRESS
        for (unsigned t = 0; t < block_words; ++t) {
            std::printf("%c = %.8x\n",
                        'A'+t, cyphertext[t]);
        }
#endif

        // Initialize working variables with block
        word_type a = cyphertext[0], b = cyphertext[1],
                  c = cyphertext[2], d = cyphertext[3];

        // Decypher block
#define BOOST_HASH_MD4_DECYPHER_STEP(aa, bb, cc, dd, fun, k, s, val) \
            { \
                word_type T = policy_type::ROTR<s>(aa); \
                aa = T \
                   - policy_type::fun(bb,cc,dd) \
                   - key[policy_type::key_index(k)] \
                   - val; \
            }
        for (unsigned t = 48; t -= 4, t >= 32; ) {
            BOOST_HASH_MD4_DECYPHER_STEP(b, c, d, a, hh, t+3, 15, 0x6ed9eba1)
            BOOST_HASH_MD4_DECYPHER_STEP(c, d, a, b, hh, t+2, 11, 0x6ed9eba1)
            BOOST_HASH_MD4_DECYPHER_STEP(d, a, b, c, hh, t+1,  9, 0x6ed9eba1)
            BOOST_HASH_MD4_DECYPHER_STEP(a, b, c, d, hh, t+0,  3, 0x6ed9eba1)

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf("Round 3: %.8x %.8x %.8x %.8x\n",
                   a, b, c, d);
#endif
        }
        for (unsigned t = 32; t -= 4, t >= 16; ) {
            BOOST_HASH_MD4_DECYPHER_STEP(b, c, d, a, gg, t+3, 13, 0x5a827999)
            BOOST_HASH_MD4_DECYPHER_STEP(c, d, a, b, gg, t+2,  9, 0x5a827999)
            BOOST_HASH_MD4_DECYPHER_STEP(d, a, b, c, gg, t+1,  5, 0x5a827999)
            BOOST_HASH_MD4_DECYPHER_STEP(a, b, c, d, gg, t+0,  3, 0x5a827999)

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf("Round 2: %.8x %.8x %.8x %.8x\n",
                   a, b, c, d);
#endif
        }
        for (unsigned t = 16; t -= 4, t < 16; ) {
            BOOST_HASH_MD4_DECYPHER_STEP(b, c, d, a, ff, t+3, 19, 0x00000000)
            BOOST_HASH_MD4_DECYPHER_STEP(c, d, a, b, ff, t+2, 11, 0x00000000)
            BOOST_HASH_MD4_DECYPHER_STEP(d, a, b, c, ff, t+1,  7, 0x00000000)
            BOOST_HASH_MD4_DECYPHER_STEP(a, b, c, d, ff, t+0,  3, 0x00000000)

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf("Round 1: %.8x %.8x %.8x %.8x\n",
                   a, b, c, d);
#endif
        }

        block_type plaintext = {{a, b, c, d}};
        return plaintext;
    }

};

} // namespace block_cyphers
} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_BLOCK_CYPHERS_MD4_HPP


/* class PFTau3ProngReco
 * EDProducer of the
 * author: Ian M. Nugent
 * The idea of the fully reconstructing the tau using a kinematic fit comes from
 * Lars Perchalla and Philip Sauerland Theses under Achim Stahl supervision. This
 * code is a result of the continuation of this work by Ian M. Nugent and Vladimir Cherepanov.
 */

#include "FWCore/Framework/interface/EventSetup.h"
#include "FWCore/Framework/interface/ESHandle.h"
#include "FWCore/Framework/interface/Event.h"
#include "FWCore/Framework/interface/MakerMacros.h"
#include "FWCore/Framework/interface/Frameworkfwd.h"
#include "FWCore/Framework/interface/EDProducer.h"
#include "FWCore/ParameterSet/interface/ParameterSet.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"
#include "FWCore/Utilities/interface/Exception.h"

#include "TrackingTools/TransientTrack/interface/TransientTrackBuilder.h"
#include "TrackingTools/Records/interface/TransientTrackRecord.h"
#include "RecoVertex/VertexPrimitives/interface/TransientVertex.h"
#include "RecoVertex/AdaptiveVertexFit/interface/AdaptiveVertexFitter.h"

#include "DataFormats/TauReco/interface/PFTau.h"
#include "DataFormats/TauReco/interface/PFTauFwd.h"
#include "DataFormats/BeamSpot/interface/BeamSpot.h"
#include "DataFormats/MuonReco/interface/Muon.h"
#include "DataFormats/MuonReco/interface/MuonFwd.h"
#include "DataFormats/VertexReco/interface/Vertex.h"
#include "DataFormats/VertexReco/interface/VertexFwd.h"
#include "DataFormats/TrackReco/interface/Track.h"
#include "DataFormats/TrackReco/interface/TrackFwd.h"
#include "DataFormats/GsfTrackReco/interface/GsfTrack.h"
#include "DataFormats/GsfTrackReco/interface/GsfTrackFwd.h"
#include "DataFormats/Common/interface/RefToBase.h"
#include "DataFormats/EgammaCandidates/interface/Electron.h"
#include "DataFormats/EgammaCandidates/interface/ElectronFwd.h"

#include "DataFormats/Common/interface/Association.h"
#include "DataFormats/Common/interface/AssociationVector.h"
#include "DataFormats/Common/interface/RefProd.h"
#include "DataFormats/TauReco/interface/PFTau3ProngSummary.h"
#include "DataFormats/TauReco/interface/PFTau3ProngSummaryFwd.h"

#include "DataFormats/TauReco/interface/PFTauDiscriminator.h"
#include "CommonTools/Utils/interface/StringCutObjectSelector.h"
#include <memory>
#include <boost/foreach.hpp>
#include <TFormula.h>

#include <memory>

#include "RecoTauTag/ImpactParameter/interface/Particle.h"
#include "RecoTauTag/ImpactParameter/interface/LorentzVectorParticle.h"
#include "RecoTauTag/ImpactParameter/interface/TrackParticle.h"
#include "RecoTauTag/ImpactParameter/interface/ParticleBuilder.h"
#include "RecoTauTag/ImpactParameter/interface/TauA1NuConstrainedFitter.h"
#include "RecoTauTag/ImpactParameter/interface/Chi2VertexFitter.h"
#include "Validation/EventGenerator/interface/PdtPdgMini.h"
#include "RecoVertex/KinematicFit/interface/KinematicParticleVertexFitter.h"
#include "RecoVertex/KinematicFitPrimitives/interface/KinematicParticleFactoryFromTransientTrack.h"
#include "RecoTauTag/ImpactParameter/interface/PDGInfo.h"
#include "TLorentzVector.h"

#include "DataFormats/HepMCCandidate/interface/GenParticle.h"

using namespace reco;
using namespace edm;
using namespace std;
using namespace tauImpactParameter;

class PFTau3ProngReco : public EDProducer {
 public:
  enum Alg{useKalmanFit=0, useTrackHelix};

  struct DiscCutPair{
    DiscCutPair():cutFormula_(nullptr){}
    ~DiscCutPair(){delete cutFormula_;}
    edm::Handle<reco::PFTauDiscriminator> handle_;
    edm::InputTag inputTag_;
    double cut_;
    TFormula* cutFormula_;
  };
  typedef std::vector<DiscCutPair*> DiscCutPairVec;

  explicit PFTau3ProngReco(const edm::ParameterSet& iConfig);
  ~PFTau3ProngReco() override;
  void produce(edm::Event&,const edm::EventSetup&) override;
 private:
  edm::InputTag PFTauTag_;
  edm::InputTag PFTauTIPTag_;
  int Algorithm_;
  DiscCutPairVec discriminators_;
  std::unique_ptr<StringCutObjectSelector<reco::PFTau> > cut_;
  int ndfPVT_;
  KinematicParticleVertexFitter kpvFitter_;
};

PFTau3ProngReco::PFTau3ProngReco(const edm::ParameterSet& iConfig):
  PFTauTag_(iConfig.getParameter<edm::InputTag>("PFTauTag")),
  PFTauTIPTag_(iConfig.getParameter<edm::InputTag>("PFTauTIPTag")),
  Algorithm_(iConfig.getParameter<int>("Algorithm")),
  ndfPVT_(iConfig.getUntrackedParameter("ndfPVT",(int)5))
{
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  std::vector<edm::ParameterSet> discriminators =iConfig.getParameter<std::vector<edm::ParameterSet> >("discriminators");
  // Build each of our cuts
  BOOST_FOREACH(const edm::ParameterSet &pset, discriminators) {
    DiscCutPair* newCut = new DiscCutPair();
    newCut->inputTag_ = pset.getParameter<edm::InputTag>("discriminator");
    if ( pset.existsAs<std::string>("selectionCut") ) newCut->cutFormula_ = new TFormula("selectionCut", pset.getParameter<std::string>("selectionCut").data());
    else newCut->cut_ = pset.getParameter<double>("selectionCut");
    discriminators_.push_back(newCut);
  }
  // Build a string cut if desired
  if (iConfig.exists("cut")) cut_ = std::make_unique<StringCutObjectSelector<reco::PFTau>>(iConfig.getParameter<std::string>("cut"));
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  produces<edm::AssociationVector<PFTauRefProd, std::vector<reco::PFTau3ProngSummaryRef> > >();
  produces<PFTau3ProngSummaryCollection>("PFTau3ProngSummary");
}

PFTau3ProngReco::~PFTau3ProngReco(){

}

void PFTau3ProngReco::produce(edm::Event& iEvent,const edm::EventSetup& iSetup){
  // Obtain Collections
  edm::ESHandle<TransientTrackBuilder> transTrackBuilder;
  iSetup.get<TransientTrackRecord>().get("TransientTrackBuilder",transTrackBuilder);

  edm::Handle<std::vector<reco::PFTau> > Tau;
  iEvent.getByLabel(PFTauTag_,Tau);

  edm::Handle<edm::AssociationVector<PFTauRefProd, std::vector<reco::PFTauTransverseImpactParameterRef> > > TIPAV;
  iEvent.getByLabel(PFTauTIPTag_,TIPAV);

  auto AVPFTau3PS = std::make_unique<edm::AssociationVector<PFTauRefProd, std::vector<reco::PFTau3ProngSummaryRef>>>(PFTauRefProd(Tau));
  auto PFTau3PSCollection_out = std::make_unique<PFTau3ProngSummaryCollection>();
  reco::PFTau3ProngSummaryRefProd PFTau3RefProd_out = iEvent.getRefBeforePut<reco::PFTau3ProngSummaryCollection>("PFTau3ProngSummary");

  // Load each discriminator
  BOOST_FOREACH(DiscCutPair *disc, discriminators_) {iEvent.getByLabel(disc->inputTag_, disc->handle_);}

  // For each Tau Run Algorithim
  if(Tau.isValid()){
    for(reco::PFTauCollection::size_type iPFTau = 0; iPFTau < Tau->size(); iPFTau++) {
      reco::PFTauRef tau(Tau, iPFTau);
      reco::PFTau3ProngSummary PFTau3PS;
      ///////////////////////
      // Check if it passed all the discrimiantors
      bool passed(true);
      BOOST_FOREACH(const DiscCutPair* disc, discriminators_) {
        // Check this discriminator passes
	bool passedDisc = true;
	if ( disc->cutFormula_ )passedDisc = (disc->cutFormula_->Eval((*disc->handle_)[tau]) > 0.5);
	else passedDisc = ((*disc->handle_)[tau] > disc->cut_);
        if ( !passedDisc ){passed = false; break;}
      }
      if (passed && cut_.get()){passed = (*cut_)(*tau);}
      if (passed){
	PDGInfo pdgInfo;
	const reco::PFTauTransverseImpactParameterRef theTIP=TIPAV->value(tau.key());
	const reco::VertexRef primaryVertex=theTIP->primaryVertex();
	/////////////////////////////////
	// Now compute the 3 prong Tau
	bool SecondaryVtxOK(false);
	LorentzVectorParticle a1;
	if(theTIP->hasSecondaryVertex() && primaryVertex->ndof()>ndfPVT_){
	  const VertexRef secVtx=theTIP->secondaryVertex();
          GlobalPoint sv(secVtx->position().x(),secVtx->position().y(),secVtx->position().z());
	  double vtxchi2(0), vtxndf(1);
	  if(useKalmanFit==Algorithm_){
	    vtxchi2=secVtx->chi2();
	    vtxndf=secVtx->ndof();
	    const std::vector<reco::Track>& selectedTracks=secVtx->refittedTracks();
	    std::vector<reco::TransientTrack> transTrkVect;
	    for(unsigned int i = 0; i!=selectedTracks.size();i++) transTrkVect.push_back(transTrackBuilder->build(selectedTracks[i]));
	    KinematicParticleFactoryFromTransientTrack kinFactory;
	    float piMassSigma(1.e-6), piChi(0.0), piNdf(0.0);
	    std::vector<RefCountedKinematicParticle> pions;
	    for(unsigned int i = 0; i<transTrkVect.size();i++) pions.push_back(kinFactory.particle(transTrkVect[i],pdgInfo.pi_mass(),piChi,piNdf,sv,piMassSigma));
	    RefCountedKinematicTree jpTree = kpvFitter_.fit(pions);
	    jpTree->movePointerToTheTop();
	    const KinematicParameters parameters = jpTree->currentParticle()->currentState().kinematicParameters();
	    AlgebraicSymMatrix77 cov=jpTree->currentParticle()->currentState().kinematicParametersError().matrix();
	    // get pions
	    double c(0);
	    std::vector<reco::Track> Tracks;
	    std::vector<LorentzVectorParticle> ReFitPions;
	    for(unsigned int i=0;i<transTrkVect.size();i++){
	      c+=transTrkVect[i].charge();
	      ReFitPions.push_back(ParticleBuilder::createLorentzVectorParticle(transTrkVect[i],*secVtx,true,true));
	    }
	    // now covert a1 into LorentzVectorParticle
	    TVectorT<double>    a1_par(LorentzVectorParticle::NLorentzandVertexPar);
	    TMatrixTSym<double> a1_cov(LorentzVectorParticle::NLorentzandVertexPar);
	    for(int i = 0; i<LorentzVectorParticle::NLorentzandVertexPar; i++){
	      a1_par(i)=parameters(i);
	      for(int j = 0; j<LorentzVectorParticle::NLorentzandVertexPar; j++){
		a1_cov(i,j)=cov(i,j);
	      }
	    }
	    a1=LorentzVectorParticle(a1_par,a1_cov,abs(PdtPdgMini::a_1_plus)*c,c,transTrackBuilder->field()->inInverseGeV(sv).z());
	    SecondaryVtxOK=true;
	    PFTau3PS=reco::PFTau3ProngSummary(theTIP,a1.p4(),vtxchi2,vtxndf);
	  }
	  else if(useTrackHelix==Algorithm_){
	    // use Track Helix
	    std::vector<TrackParticle> pions;
	    GlobalPoint pvpoint(primaryVertex->position().x(),primaryVertex->position().y(),primaryVertex->position().z());
	    const std::vector<edm::Ptr<reco::PFCandidate> > cands = tau->signalPFChargedHadrCands();
	    for (std::vector<edm::Ptr<reco::PFCandidate> >::const_iterator iter = cands.begin(); iter!=cands.end(); ++iter) {
	      if(iter->get()->trackRef().isNonnull()){
		reco::TransientTrack transTrk=transTrackBuilder->build(iter->get()->trackRef());
		pions.push_back(ParticleBuilder::createTrackParticle(transTrk,pvpoint,true,true));
	      }
	      else if(iter->get()->gsfTrackRef().isNonnull()){
		//reco::TransientTrack transTrk=transTrackBuilder->build(iter->get()->gsfTrackRef());
		//pions.push_back(ParticleBuilder::CreateTrackParticle(transTrk,pvpoint,true,true));
	      }
	    }
	    TVector3 pv(secVtx->position().x(),secVtx->position().y(),secVtx->position().z());
	    Chi2VertexFitter chi2v(pions,pv);
	    SecondaryVtxOK=chi2v.fit();
	    double c(0); for(unsigned int i=0;i<pions.size();i++){c+=pions[i].charge();}
	    int pdgid=abs(PdtPdgMini::a_1_plus)*c;
	    a1=chi2v.getMother(pdgid);
	    PFTau3PS =reco::PFTau3ProngSummary(theTIP,a1.p4(),vtxchi2,vtxndf);
	  }
	}
	if(SecondaryVtxOK){
	  // Tau Solver
	  TVector3 pv(primaryVertex->position().x(),primaryVertex->position().y(),primaryVertex->position().z());
	  TMatrixTSym<double> pvcov(LorentzVectorParticle::NVertex);
	  math::Error<LorentzVectorParticle::NVertex>::type pvCov;
	  primaryVertex->fill(pvCov);
	  for(int i = 0; i<LorentzVectorParticle::NVertex; i++){
	    for(int j = 0; j<LorentzVectorParticle::NVertex; j++){
	      pvcov(i,j)=pvCov(i,j);
	    }
	  }
	  for(unsigned int i=0; i<PFTau3ProngSummary::nsolutions;i++){
	    TauA1NuConstrainedFitter TauA1NU(i,a1,pv,pvcov);
	    bool isFitOK=TauA1NU.fit();
	    if(isFitOK){
	      LorentzVectorParticle theTau=TauA1NU.getMother();
	      std::vector<LorentzVectorParticle> daughter=TauA1NU.getRefitDaughters();
	      std::vector<TLorentzVector> daughter_p4;
	      std::vector<int> daughter_charge,daughter_PDGID;
	      for(unsigned int d=0;d<daughter.size();d++){
		daughter_p4.push_back(daughter[d].p4());
		daughter_charge.push_back((int)daughter[d].charge());
		daughter_PDGID.push_back(daughter[d].pdgId());
	      }
	      PFTau3PS.AddSolution(i,theTau.p4(),daughter_p4,daughter_charge,daughter_PDGID,(isFitOK),0.0,-999);
	    }
	  }
	}
      }
      reco::PFTau3ProngSummaryRef PFTau3PSRef=reco::PFTau3ProngSummaryRef(PFTau3RefProd_out,PFTau3PSCollection_out->size());
      PFTau3PSCollection_out->push_back(PFTau3PS);
      AVPFTau3PS->setValue(iPFTau,PFTau3PSRef);
    }
  }
  iEvent.put(std::move(PFTau3PSCollection_out),"PFTau3ProngSummary");
  iEvent.put(std::move(AVPFTau3PS));
}

DEFINE_FWK_MODULE(PFTau3ProngReco);


#define BOOST_AUTO_TEST_MAIN integrators
#include <string>
#include <boost/test/unit_test.hpp>

#include "prx/planning/world_model.hpp"

// Using acrobot due to its trajectories diverging fasts
// when changing integration step
#include "prx/simulation/plants/two_link_acrobot.hpp"

BOOST_AUTO_TEST_CASE( integrators )
{
    auto acrobot_euler = prx::system_factory_t::create_system("Acrobot", "Acrobot_euler");
    auto acrobot_rk4 = prx::system_factory_t::create_system("Acrobot", "Acrobot_rk4");
    auto acrobot_dorpi = prx::system_factory_t::create_system("Acrobot", "Acrobot_dorpi");

     std::dynamic_pointer_cast<prx::plant_t>(acrobot_euler) -> set_integrator(prx::integrator_t::kEULER);
     std::dynamic_pointer_cast<prx::plant_t>(acrobot_rk4) -> set_integrator(prx::integrator_t::kRK4);
     std::dynamic_pointer_cast<prx::plant_t>(acrobot_dorpi) -> set_integrator(prx::integrator_t::kDOPRI5);

    std::vector<std::vector<double> > ctrls = {{6.94678,0.12,0.12},
                                               {6.70279,0.28,0.4},
                                               {6.22159,0.33,0.73},
                                               {-3.61730,0.36,1.09},
                                               {-5.02736,0.23,1.32},
                                               {-1.41110,0.2,1.52},
                                               {-6.98014,0.43,1.95},
                                               {-6.73070,0.2,2.15},
                                               {-3.74701,0.1,2.25},
                                               {2.47204,0.29,2.54},
                                               {3.82880,0.37,2.91},
                                               {-4.03203,0.01,2.92},
                                               {-2.47204,0.43,3.35}};

    prx::distance_function_t dist = [](prx::space_point_t a, prx::space_point_t b)
    {
        double res = 0;
        for (int i = 0; i < a -> get_dim(); ++i)
        {
            res += std::pow( a -> at(i) - b -> at(i) , 2);
        }
        return std::sqrt(res);
    };

    prx::world_model_t<> world_model({acrobot_euler},{});
    world_model.create_context("disk_context",{"Acrobot_euler"},{});
    auto context = world_model.get_context("disk_context");
    auto sg = context.first;
    auto state_space = context.first->get_state_space();
    auto control_space = context.first->get_control_space();

    prx::space_point_t curr_state;
    prx::plan_t plan_euler(control_space);
    prx::trajectory_t euler_traj(state_space);

    prx::world_model_t<> world_model_rk4({acrobot_rk4},{});
    world_model_rk4.create_context("context_rk4",{"Acrobot_rk4"},{});
    auto context_rk4 = world_model_rk4.get_context("context_rk4");
    auto sg_rk4 = context.first;
    auto state_space_rk4 = context.first->get_state_space();
    auto control_space_rk4 = context.first->get_control_space();

    prx::plan_t plan_rk4(control_space_rk4);
    prx::trajectory_t traj_rk4(state_space_rk4);

    prx::world_model_t<> world_model_dorpi({acrobot_dorpi},{});
    world_model_dorpi.create_context("context_dorpi",{"Acrobot_dorpi"},{});
    auto context_dorpi = world_model_dorpi.get_context("context_dorpi");
    auto sg_dorpi = context.first;
    auto state_space_dorpi = context_dorpi.first->get_state_space();
    auto control_space_dorpi = context_dorpi.first->get_control_space();

    prx::plan_t plan_dorpi(control_space_dorpi);
    prx::trajectory_t traj_dorpi(state_space_dorpi);

    for (auto ctrl : ctrls)
    {
        plan_euler.append_onto_back(ctrl[1]);
        plan_euler.back().control -> at(0) = ctrl[0];

        plan_rk4.append_onto_back(ctrl[1]);
        plan_rk4.back().control -> at(0) = ctrl[0];

        plan_dorpi.append_onto_back(ctrl[1]);
        plan_dorpi.back().control -> at(0) = ctrl[0];
    }

    curr_state = state_space -> make_point();
    curr_state -> at(0) = 0.0;
    curr_state -> at(1) = 0.0;
    curr_state -> at(2) = 0.0;
    curr_state -> at(3) = 0.0;

    prx::simulation_step=0.000001;
    sg -> propagate(curr_state, plan_euler, euler_traj);
    prx::simulation_step=0.001;
    sg_rk4 -> propagate(curr_state, plan_rk4, traj_rk4);

    prx::simulation_step=0.001;
    sg_dorpi -> propagate(curr_state, plan_dorpi, traj_dorpi);

    for (double i = 0; i <= 1; i+=0.01)
    {
        auto pt_euler = euler_traj.at(i);
        auto pt_rk4 = traj_rk4.at(i);
        auto pt_dorpi = traj_dorpi.at(i);

        BOOST_CHECK(dist(pt_rk4, pt_euler) < 0.1);
        BOOST_CHECK(dist(pt_rk4, pt_dorpi) < 0.01);
        // std::cout << "euler: " << pt_euler << "\trk4: " << pt_rk4 << std::endl;
        // std::cout << "dorpi: " << pt_dorpi << "\trk4: " << pt_rk4 << std::endl;
    }

}


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

#include <boost/hana/assert.hpp>
#include <boost/hana/chain.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/for_each.hpp>
#include <boost/hana/traits.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <type_traits>
#include <utility>
namespace hana = boost::hana;

// Using the `tuple` Monad, we generate all the possible combinations of
// cv-qualifiers and reference qualifiers. Then, we use the `optional`
// Monad to make sure that our generic function can be called with
// arguments of any of those types.

// cv_qualifiers : type -> tuple(type)
auto cv_qualifiers = [](auto t) {
    return hana::make_tuple(
        t,
        hana::traits::add_const(t),
        hana::traits::add_volatile(t),
        hana::traits::add_volatile(hana::traits::add_const(t))
    );
};

// ref_qualifiers : type -> tuple(type)
auto ref_qualifiers = [](auto t) {
    return hana::make_tuple(
        hana::traits::add_lvalue_reference(t),
        hana::traits::add_rvalue_reference(t)
    );
};

auto possible_args = cv_qualifiers(hana::type_c<int>) | ref_qualifiers;

BOOST_HANA_CONSTANT_CHECK(
    possible_args == hana::make_tuple(
                        hana::type_c<int&>,
                        hana::type_c<int&&>,
                        hana::type_c<int const&>,
                        hana::type_c<int const&&>,
                        hana::type_c<int volatile&>,
                        hana::type_c<int volatile&&>,
                        hana::type_c<int const volatile&>,
                        hana::type_c<int const volatile&&>
                    )
);

struct some_function {
    template <typename T>
    void operator()(T&&) const { }
};

int main() {
    hana::for_each(possible_args, [](auto t) {
        using T = typename decltype(t)::type;
        static_assert(decltype(hana::is_valid(some_function{})(std::declval<T>())){},
        "some_function should be callable with any type of argument");
    });
}


#ifndef cloth_sim_object_hpp
#define cloth_sim_object_hpp

#include <elasty/sim-object.hpp>
#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace elasty
{
    class ClothSimObject : public SimObject
    {
    public:

        enum class Strategy
        {
            Bending,
            IsometricBending,
            Cross,
        };

        using TriangleList = Eigen::Matrix<int32_t, Eigen::Dynamic, 3, Eigen::RowMajor>;

        ClothSimObject(const std::string& obj_path,
                       const double distance_stiffness = 0.90,
                       const double bending_stiffness = 0.50,
                       const Eigen::Affine3d& transform = Eigen::Affine3d::Identity(),
                       const Strategy strategy = Strategy::IsometricBending);

        TriangleList m_triangle_list;
    };
}

#endif /* cloth_sim_object_hpp */


/*
 * Copyright 2019-2020 Tomasz bla Fortuna. All rights reserved.
 * License: MIT
 * bla@thera.be, https://github.com/blaa/flatritrie
 */

#ifndef _TRIE_H_
#define _TRIE_H_

#include <iostream>
#include <string>
#include <vector>
#include <bitset>
#include <boost/algorithm/string.hpp>

#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

/*
 * Basic Trie with 1 bit on a path. Browsing requires random jumps over a memory
 * and therefore doesn't play nice with caches.
 *
 * Would work for IPv6. The Trie is browsed until the end and the best "value"
 * match is updated, so we can match the best matching entry.
 *
 * This is used as a reference in benchmarking.
 */
class Trie {
protected:
    struct Node {
        /* 0 for bit 0, 1 for bit 1 */
        Node *child[2];
        /* -1 for middle node */
        int id;

        Node() : child(), id(-1) {}

        void show() {
            std::cout << "Node id="
                      << this->id
                      << " left=" << (this->child[0] != NULL)
                      << " right=" << (this->child[1] != NULL)
                      << std::endl;
        }
    };

    Node root;
    int nodes_cnt = 0;

    Node *get_or_create(Node *cur, const int bit) {
        if (cur->child[bit] == NULL) {
            cur->child[bit] = new Node();
            this->nodes_cnt += 1;
        }
        return cur->child[bit];
    }

    void add_ip(uint32_t ip, int32_t mask, int id) {
        int mask_left = mask;
        Node *cur = &this->root;

        while (mask_left > 0) {
            const int bit = ip >> 31;
            cur = this->get_or_create(cur, bit);

            /* Next one! */
            ip <<= 1;
            mask_left--;
        }

        /* After using whole mask, the IP should be 0 */
        assert(ip == 0);

        /* No duplicates at the same level are acceptable */
        if (cur->id != -1) {
            std::cout << "New ID " << id << " collided with existing "
                      << cur->id << std::endl;
            throw new std::exception();
        }
        assert(cur->id == -1);
        cur->id = id;
    }

    void show_tree(Node *cur, int indent) {
        const std::string indent_s = std::string(indent, ' ');
        std::cout << indent_s << "Node: " << cur->id << std::endl;
        if (cur->child[0]) {
            std::cout << indent_s << "0: SUB" << std::endl;
            this->show_tree(cur->child[0], indent + 3);
        } else {
            std::cout << indent_s << "0: NULL" << std::endl;
        }
        if (cur->child[1]) {
            std::cout << indent_s << "1: SUB" << std::endl;
            this->show_tree(cur->child[1], indent + 3);
        } else {
            std::cout << indent_s << "1 NULL" << std::endl;
        }
    }

public:
    Trie() {}

    void add(const std::string &addr, int id) {
        std::vector<std::string> addr_mask;
        boost::split(addr_mask, addr, boost::is_any_of("/"));
        assert(addr_mask.size() == 2);

        const int mask = atoi(addr_mask[1].c_str());
        in_addr ip_parsed;
        int ret = inet_aton(addr_mask[0].c_str(), &ip_parsed);
        if (ret == 0)
            throw std::exception();
        assert(mask >= 0 && mask <= 32);

        uint32_t ip_network = ntohl(ip_parsed.s_addr);
        this->add_ip(ip_network, mask, id);
    }

    int query_string(const std::string &addr) {
        in_addr ip_parsed;
        int ret = inet_aton(addr.c_str(), &ip_parsed);
        if (ret == 0)
            throw std::exception();
        assert(ret != 0);
        uint32_t ip_network = ntohl(ip_parsed.s_addr);
        return this->query(ip_network);
    }

    int query(uint32_t ip) const {
        const Node *cur = &this->root;
        int matched_id = cur->id;
        for (int mask = 0; mask < 32; mask++) {
            const int bit = ip >> 31;
            cur = cur->child[bit];
            if (cur == NULL) {
                break;
            }
            if (cur->id != -1) {
                matched_id = cur->id;
                /* We will continue search, as there might be a closer match */
            }
            ip <<= 1;
        }
        return matched_id;
    }

    void show() {
        this->show_tree(&this->root, 0);
    }

    int size() const {
        return this->nodes_cnt;
    }

    friend class FlaTrie;
};

/*
 * Structure built from the existing Trie which optimizes it for fast memory accesses:
 * - Consecutive 0, or 1 bits are compressed into a single entry.
 * - Kinda similar to Radix Trie.
 * - Data are flatten and stored in a table with jumps. Might be better for cache.
 * - Idea is to create "finite state automata" for browsing a Trie efficiently.
 */
class FlaTrie {
private:
    struct Entry;

    struct Side {
        Entry *jump = NULL;
        /* Number of bits (0 or 1) required to match */
        int16_t count = -1;

        Side() {}
        Side(int count, Entry *jump) : jump(jump), count(count) {}
    };

    struct Entry {
        /* Displaying only */
        int pos;

        /* Value (id) if reached this table entry */
        int id = -1;

        /* Two children: child[0] for bit 0, and [1] for bit 1 */
        Side child[2];

        void show(int pos) {
            std::cout << "entry=" << pos
                      << " id=" << this->id
                      << " 0x" << (int)this->child[0].count << "->"
                      << (this->child[0].jump == NULL ? -1 : this->child[0].jump->pos)
                      << " 1x" << (int)this->child[1].count << "->"
                      << (this->child[1].jump == NULL ? -1 : this->child[1].jump->pos)
                      << std::endl;
        }
    };

    Entry *table = 0;
    int used = 0;

    Side build_node(Trie::Node *node, int current_bit) {
        if (node == NULL) {
            /* Reached the end of the path */
            return Side(0, NULL);
        }

        const int other_bit = !current_bit;

        /* ID node or possible crossing - new node. */
        if (node->id != -1 || node->child[other_bit] != NULL) {
            Entry &entry = this->table[this->used];
            assert(entry.id == -1);
            entry.pos = this->used;
            entry.id = node->id;
            this->used++;

            auto left_path = build_node(node->child[0], 0);
            auto right_path = build_node(node->child[1], 1);
            entry.child[0] = left_path;
            entry.child[1] = right_path;
            return Side(1, &entry);
        }

        /* Create nodes for crossings or for IDs */
        if (current_bit == 0) {
            /* Continue 0 path - no node */
            assert(node->child[1] == NULL);
            auto path = build_node(node->child[0], 0);
            return Side(path.count + 1, path.jump);
        } else {
            /* Continue 1 path - no node */
            assert(node->child[0] == NULL);
            auto path = build_node(node->child[1], 1);
            return Side(path.count + 1, path.jump);
        }
    }
public:
    /* Build flatrie from a trie */
    void build(const Trie &trie) {
        delete[] this->table;
        /* NOTE: Memory management here is rather simplified. */
        this->table = new Entry[trie.size()];
        used = 1;
        Entry &entry = this->table[0];

        auto path = build_node(trie.root.child[0], 0);
        entry.child[0] = path;

        path = build_node(trie.root.child[1], 1);
        entry.child[1] = path;

        std::cout << "Used " << this->used
                  << " entries in the Flatrie table" << std::endl;
    }

    int query_string(const std::string &addr) {
        in_addr ip_parsed;
        int ret = inet_aton(addr.c_str(), &ip_parsed);
        assert(ret != 0);
        if (ret == 0)
            throw std::exception();
        uint32_t ip_network = ntohl(ip_parsed.s_addr);
        return this->query(ip_network);
    }

    /* Query by network byte order IP */
    int query(uint32_t ip) const {
        const Entry *cur = &this->table[0];
        int matched_id = -1;
        int hops = 0;

        for (;;) {
            const int bit = ip >> 31;
            auto &side = cur->child[bit];

            if (side.jump == NULL) {
                /* Nowhere to run */
                return matched_id;
            }

            /* Skip X bits */
            /* TODO: clz */
            for (int i = 0; i < side.count; i++) {
                const int cur_bit = ip >> 31;
                if (cur_bit != bit) {
                    return matched_id;
                }
                ip <<= 1;
            }
            cur = side.jump;
            hops += 1;
            if (cur->id != -1) {
                matched_id = cur->id;
            }
            continue;
        }
        return matched_id;
    }

    void show() {
        for (int i = 0; i < this->used; i++) {
            Entry &entry = this->table[i];
            entry.show(i);
        }
    }
};

#endif


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

namespace boost { namespace simd { namespace tag
  {
   /*!
     @brief modf generic tag

     Represents the modf function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct modf_ : ext::elementwise_<modf_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<modf_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_modf_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::modf_, Site> dispatching_modf_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::modf_, Site>();
   }
   template<class... Args>
   struct impl_modf_;
  }
  /*!
    Returns the integer and the fractionnal part of the input

    @par Semantic:

    @code
    tie(t, f) = modf(x);
    @endcode

    is similar to:

    @code
    T t = trunc(x);
    T r = frac(x);
    @endcode

    @param a0 Value to decompose

    @return  A pair containing the integer and fractionnal part of @c a0
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::modf_, modf, 1)

  /*!
    Computes the integer and the fractionnal part of the input

    @par Semantic:

    @code
    f = modf(x, t);
    @endcode

    is similar to:

    @code
    T t = trunc(x);
    T f = frac(x);
    @endcode

    @param a0 Value to decompose
    @param a1 L-Value that will receive the fractionnal part of a0

    @return a value of same type as the input
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL(tag::modf_, modf,(A0 const&)(A1&),2)
  /*!
    Computes the integer and the fractionnal part of the input

    @par Semantic:

    @code
    modf(x, t, f);
    @endcode

    is similar to:

    @code
    T t = trunc(x);
    T f = frac(x);
    @endcode

    @param a0 Value to decompose
    @param a1 L-Value that will receive the fractionnal part of @c a0
    @param a2 L-Value that will receive the integer part of @c a0

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL(tag::modf_, modf,(A0 const&)(A1&)(A1&),2)

} }

#endif


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/lexical_cast.hpp>
#include<sstream>

#include "formast.hpp"
#include "printer.hpp"

// disable indentation and newlines for simpler inline tests
class TestPrinter : public formast::Printer
{
public:
    TestPrinter(std::ostream & os) : formast::Printer(os) {};
    void indent() {};
    void open_tag(std::string const & tag, bool final) {
        os << "<" << tag << ">";
    };
    void close_tag(std::string const & tag, bool final) {
        os << "</" << tag << ">";
    };
};

class ParseFixture
{
public:
    ParseFixture() {};
    void parse_check(std::string const & in, std::string const & out) const {
        formast::Module ast;
        formast::XmlParser parser;
        std::string xmlin =
            "<niftoolsxml><compound name=\"Test\"><add type=\"uint\" name=\"test\" cond=\""
            + in
            + "\"></add></compound></niftoolsxml>";
        std::string xmlout =
            "<class><name>Test</name><stats><if><expr>"
            + out
            + "</expr><then><stats><field><type>uint</type><name>test</name></field></stats></then></if></stats></class>";
        BOOST_CHECK_NO_THROW(parser.parse_string(xmlin, ast));

        std::ostringstream os;
        TestPrinter print(os);
        print.module(ast);
        BOOST_CHECK_EQUAL(os.str(), xmlout);
    };
};

BOOST_AUTO_TEST_SUITE(expr_parse_xml_test_suite)

BOOST_FIXTURE_TEST_CASE(uint_test, ParseFixture)
{
    parse_check("31415", "<uint>31415</uint>");
}

BOOST_FIXTURE_TEST_CASE(id_test, ParseFixture)
{
    parse_check("Test Me + 3", "<add><id>Test Me</id><uint>3</uint></add>");
}

BOOST_FIXTURE_TEST_CASE(unary_test, ParseFixture)
{
    parse_check("+31415", "<pos><uint>31415</uint></pos>");
    parse_check("-31415", "<neg><uint>31415</uint></neg>");
}

BOOST_FIXTURE_TEST_CASE(binary_test, ParseFixture)
{
    parse_check("1+2", "<add><uint>1</uint><uint>2</uint></add>");
    parse_check("3-4", "<uint>3</uint><uint>4</uint>");
    parse_check("7*8", "<mul><uint>7</uint><uint>8</uint></mul>");
    parse_check("19/33", "
<uint>19</uint><uint>33</uint>
");
}

BOOST_FIXTURE_TEST_CASE(complex_test, ParseFixture)
{
    parse_check("1*2+6/3*7-9*(1+2-3-4)", "<add><mul><uint>1</uint><uint>2</uint></mul><mul>
<uint>6</uint><uint>3</uint>
<uint>7</uint></mul></add><mul><uint>9</uint><add><uint>1</uint><uint>2</uint></add><uint>3</uint><uint>4</uint></mul>");
}

BOOST_FIXTURE_TEST_CASE(complex2_test, ParseFixture)
{
    parse_check("(User Version >= 10) || ((User Version == 1) && (Version != 10.2.0.0))", "<logical_or><compare_ge><id>User Version</id><uint>10</uint></compare_ge><logical_and><compare_eq><id>User Version</id><uint>1</uint></compare_eq><compare_ne><id>Version</id><uint>167903232</uint></compare_ne></logical_and></logical_or>");
}

BOOST_AUTO_TEST_SUITE_END()