#include <iostream>
#include <random>
#include <string>
#include <algorithm>
#include <vector>
#include <unordered_map>
#include <boost/python.hpp>
#include <boost/progress.hpp>

namespace py = boost::python;

using namespace std;

class GibbsSamplingDMM {
    public:
        int niters, num_documents, num_of_words_in_corpus, ntopics, top_words, vocabulary_size;;
        double alpha, alpha_sum,  beta, beta_sum;
        bool verbose;

        vector<int> _topic_assignments, _sum_topic_word_count, doc_topic_count, _topics_convergence;
        vector<double> multi_pros;
        vector<vector<int>> _topic_word_count, occurence_to_index_count, corpus;
        unordered_map<string, int> word_to_id_vocabulary;
        unordered_map<int, string> _id_to_word_vocabulary;

        py::list documents;

        GibbsSamplingDMM(py::list documents, double alpha=0.1, double beta=0.01, int ntopics=20, int niters=2000, int top_words=20, bool verbose=0) {
            this->generator();
            this->documents = documents;
            this->alpha = alpha;
            this->beta = beta;
            this->ntopics = ntopics;
            this->niters = niters;
            this->top_words = top_words;
            this->verbose = verbose;
        };

        void fit() {
            cout << "Running Gibbs sampling inference";
            boost::progress_display show_progress(this->niters);
            for(int i=1; i < this->niters+1; i++) {
                this->single_iteration();
                this->_topics_convergence.push_back(this->ntopics-count(this->_sum_topic_word_count.begin(), this->_sum_topic_word_count.end(), 0));
                ++show_progress;
            }
        }

        void analyse_corpus() {
            this->num_of_words_in_corpus = 0;
            int index_word = -1;
            for(int i = 0; i < len(this->documents); i++){
                vector<int> current_doc, word_occurence_to_index_doc;
                unordered_map<string, int> word_occurence_to_index_doc_count;
                py::list words = py::extract<py::list>(this->documents[i]);
                for(int j = 0; j <len(words); j++) {
                    string word = py::extract<string>(words[j]);
                    if(this->word_to_id_vocabulary.find(word) == this->word_to_id_vocabulary.end()){
                        index_word += 1;
                        this->word_to_id_vocabulary[word] = index_word;
                        this->_id_to_word_vocabulary[index_word] = word;
                    }
                    current_doc.push_back(this->word_to_id_vocabulary[word]);
                    if(word_occurence_to_index_doc_count.find(word) != this->word_to_id_vocabulary.end()){
                        word_occurence_to_index_doc_count[word] += 1;
                    } else {
                        word_occurence_to_index_doc_count[word] = 1;
                    }
                    word_occurence_to_index_doc.push_back(word_occurence_to_index_doc_count[word]);
                }
                this->corpus.push_back(current_doc);
                this->num_of_words_in_corpus += len(words);
                this->occurence_to_index_count.push_back(word_occurence_to_index_doc);
            }
            this->vocabulary_size = this->word_to_id_vocabulary.size();
            this->doc_topic_count = vector<int> (this->ntopics, 0);
            this->_topic_word_count = vector<vector<int>> (this->ntopics, vector<int> (this->vocabulary_size, 0));
            this->_sum_topic_word_count = vector<int> (this->ntopics, 0);
            this->multi_pros = vector<double> (this->ntopics, 1.0/this->ntopics);
            this->num_documents = len(this->documents);
            this->beta_sum = this->vocabulary_size * this->beta;
            this->alpha_sum = this->alpha * this->ntopics;
            if(this->verbose == 1) {
                cout << "Corpus size: " << this->num_documents << " docs, " << this->num_of_words_in_corpus << " words \n";
                cout << "Vocabulary size: " << this->vocabulary_size << "\n";
                cout << "Number of topics: " << this->ntopics << "\n";
                cout << "alpha: " << this->alpha << "\n";
                cout << "beta: " << this->beta  << "\n";
                cout << "Number of sampling iterations: " << this->niters << "\n";
                cout << "Number of top topical words: " << this->top_words << "\n";

            }
        }

        void initialize_topic_assignments() {
            for(int i = 0; i < this->num_documents; i++){
                int topic = this->next_discrete(this->multi_pros);
                this->doc_topic_count[topic] += 1;
                int doc_size = this->corpus[i].size();
                for(int j = 0; j < doc_size; j++) {
                    int word = this->corpus[i][j];
                    this->_topic_word_count[topic][word] += 1;
                    this->_sum_topic_word_count[topic] += 1;
                }
                this->_topic_assignments.push_back(topic);
            }
        }

        py::list predict(py::list unseen_docs, int niters=100, bool probs=1) {

            int num_documents = len(unseen_docs), num_of_words_in_corpus = 0;

            vector<int> topic_assignments, doc_topic_count (this->ntopics, 0);
            vector<double> multi_pros (this->ntopics, 1.0/this->ntopics);
            vector<vector<int>> corpus, occurence_to_index_count;

            py::list results;

            for(int i = 0; i < num_documents; i++) {
                vector<int> current_doc, word_occurence_to_index_doc;
                unordered_map<string, int> word_occurence_to_index_doc_count;
                py::list words = py::extract<py::list>(unseen_docs[i]);
                for(int j = 0; j <len(words); j++) {
                    string word = py::extract<string>(words[j]);
                    if(this->word_to_id_vocabulary.find(word) != this->word_to_id_vocabulary.end()){
                        current_doc.push_back(this->word_to_id_vocabulary[word]);
                        if(word_occurence_to_index_doc_count.find(word) == this->word_to_id_vocabulary.end()){
                            word_occurence_to_index_doc_count[word] = 1;
                        } else {
                             word_occurence_to_index_doc_count[word] +=  1;
                        }
                        word_occurence_to_index_doc.push_back(word_occurence_to_index_doc_count[word]);
                    }
                }
                num_of_words_in_corpus += current_doc.size();
                corpus.push_back(current_doc);
                occurence_to_index_count.push_back(word_occurence_to_index_doc);
            }

            for(int i = 0; i < num_documents; i++){
                int topic = this->next_discrete(multi_pros);
                doc_topic_count[topic] += 1;
                int doc_size = corpus[i].size();
                for(int j = 0; j < doc_size; j++) {
                    int word = corpus[i][j];
                    this->_topic_word_count[topic][word] += 1;
                    this->_sum_topic_word_count[topic] += 1;
                }
                topic_assignments.push_back(topic);
            }

            for(int i = 0; i < niters; i++) {
                for(int d_index = 0; d_index < num_documents; d_index++) {
                    int topic = topic_assignments[d_index];
                    vector<int> document = corpus[d_index];
                    int doc_size = document.size();
                    doc_topic_count[topic] -= 1;

                    for(int w_index = 0; w_index < doc_size; w_index++) {
                        int word = document[w_index];
                        this->_topic_word_count[topic][word] -= 1;
                        this->_sum_topic_word_count[topic] -= 1;
                    }

                    for(int t_index = 0; t_index < this->ntopics; t_index++) {
                        multi_pros[t_index] = doc_topic_count[t_index] + this->alpha;

                        for(int w_index = 0; w_index < doc_size; w_index++) {
                            int word = document[w_index];
                            double num = this->_topic_word_count[t_index][word] + this->beta + occurence_to_index_count[d_index][w_index] - 1;
                            double den = this->_sum_topic_word_count[t_index] + this->beta + w_index;
                            multi_pros[t_index] *= num/den;
                        }
                    }

                    topic = this->next_discrete(multi_pros);
                    doc_topic_count[topic] += 1;

                    for(int w_index = 0; w_index < doc_size; w_index++) {
                        int word = document[w_index];
                        this->_topic_word_count[topic][word] += 1;
                        this->_sum_topic_word_count[topic] += 1;
                    }

                    topic_assignments[d_index] = topic;

                }
            }

            if(probs) {

                for(int i = 0; i < num_documents; i++) {
                    py::dict probs_topics;
                    int doc_size = corpus[i].size();
                    double sum_ = 0;

                    for(int t_index = 0; t_index < this->ntopics; t_index++) {
                        multi_pros[t_index] = doc_topic_count[t_index] + this->alpha;

                        for(int w_index = 0; w_index < doc_size; w_index++){
                            int word = corpus[i][w_index];
                            multi_pros[t_index] *= (this->_topic_word_count[t_index][word] + this->beta)/(this->_sum_topic_word_count[t_index] + this->beta_sum);
                        }

                        sum_ += multi_pros[t_index];
                    }

                    for(int t_index = 0; t_index < this->ntopics; t_index++) {
                        double res = multi_pros[t_index];
                        double probs_ = res / sum_;
                        probs_topics[t_index] = probs_;
                    }
                    results.append(probs_topics);
                }
                return results;
            }
            return toPythonList(topic_assignments);
        }

        py::list topic_assignments() {
            return toPythonList(this->_topic_assignments);
        }

        py::list topic_word_count() {
            return toPythonListOfList(this->_topic_word_count);
        }

        py::list sum_topic_word_count() {
            return toPythonList(this->_sum_topic_word_count);
        }

        py::list topics_convergence() {
            return toPythonList(this->_topics_convergence);
        }

        py::dict id_to_word_vocabulary() {
            return toPythonDict(this->_id_to_word_vocabulary);
        }

        template <class K, class V>
        static py::dict toPythonDict(unordered_map<K, V> map) {
            typename unordered_map<K, V>::iterator iter;
            py::dict dictionary;
            for (iter = map.begin(); iter != map.end(); ++iter) {
                dictionary[iter->first] = iter->second;
            }
            return dictionary;
        }

        template <class T>
        static py::list toPythonList(vector<T> vector) {
            typename vector<T>::iterator iter;
            py::list list;
            for (iter = vector.begin(); iter != vector.end(); ++iter) {
                list.append(*iter);
            }
            return list;
        }

        static py::list toPythonListOfList(vector<vector<int>> vector) {
            py::list list;
            for (int i = 0; i<vector.size(); i++) {
                list.append(toPythonList(vector[i]));
            }
            return list;
        }

        static vector<int> toStdVectorInt(py::object list) {
            vector<int> res;
            for(int i = 0; i<len(list); i++) {
                res.push_back(py::extract<int>(list[i]));
            }
            return res;
        }

        static vector<vector<int>> toStdVectorOfVectorInt(py::object list) {
            vector<vector<int>> res;
            for(int i = 0; i<len(list); i++) {
                res.push_back(toStdVectorInt(list[i]));
            }
            return res;
        }

        static vector<double> toStdVectorDouble(py::object list) {
            vector<double> res;
            for(int i = 0; i<len(list); i++) {
                res.push_back(py::extract<double>(list[i]));
            }
            return res;
        }

        static unordered_map<string, int> toMapStrInt(py::object obj){
            py::dict dict = py::extract<py::dict>(obj);
            unordered_map<string, int> res;
            py::list keys = dict.keys();
            for(int i = 0; i < len(keys); i++) {
                string my_key = py::extract<string>(keys[i]);
                int value = py::extract<int>(dict[my_key]);
                res[my_key] = value;
            }
            return res;
        }

        static unordered_map<int, string> toMapIntStr(py::object obj){
            py::dict dict = py::extract<py::dict>(obj);
            unordered_map<int, string> res;
            py::list keys = dict.keys();
            for(int i = 0; i < len(keys); i++) {
                int my_key = py::extract<int>(keys[i]);
                string value = py::extract<string>(dict[my_key]);
                res[my_key] = value;
            }
            return res;
        }

    private:
        default_random_engine generator;

        int next_discrete(vector<double> probs) {
            uniform_real_distribution<double> distribution(0.0, 1.0);
            double sum_ = 0;
            for(int i = 0; i < probs.size(); i++) {
                sum_ += probs[i];
            }
            double r_unif = distribution(this->generator);
            double r = sum_*r_unif;
            sum_ = 0;
            for(int i = 0; i <probs.size(); i++) {
                sum_ += probs[i];
                if(sum_ > r) {
                    return(i);
                }
            }
            return (probs.size()-1);
        }

        void single_iteration() {
            for(int d_index = 0; d_index < this->num_documents; d_index++){
                int topic = this->_topic_assignments[d_index];
                vector<int> document = this->corpus[d_index];
                int doc_size = document.size();

                this->doc_topic_count[topic] -= 1;

                for(int w_index = 0; w_index < doc_size; w_index++){
                    int word = document[w_index];
                    this->_topic_word_count[topic][word] -= 1;
                    this->_sum_topic_word_count[topic] -= 1;
                }

                for(int t_index = 0; t_index <- this->ntopics; t_index++){

                    this->multi_pros[t_index] = this->doc_topic_count[t_index] + this->alpha;

                    for(int w_index = 0; w_index < doc_size; w_index++){
                        int word = document[w_index];
                        double num = this->_topic_word_count[t_index][word] + this->beta + this->occurence_to_index_count[d_index][w_index] - 1;
                        double den = this->_sum_topic_word_count[t_index] + this->beta_sum + w_index;
                        this->multi_pros[t_index] *= num/den;
                    }

                }

                topic = this->next_discrete(this->multi_pros);
                this->doc_topic_count[topic] += 1;

                for(int w_index = 0; w_index < doc_size; w_index++){
                    int word = document[w_index];
                    this->_topic_word_count[topic][word] += 1;
                    this->_sum_topic_word_count[topic] += 1;
                }

                this->_topic_assignments[d_index] = topic;

            }
        }

};

struct gibbs_sampling_dmm_suite : py::pickle_suite {
    static py::tuple getinitargs(const GibbsSamplingDMM& g) {
        return py::make_tuple(g.documents, g.alpha, g.beta, g.ntopics, g.niters, g.top_words, g.verbose);
    }

    static py::tuple getstate(const GibbsSamplingDMM& g) {

        py::list py_topic_assignments = g.toPythonList(g._topic_assignments),
            py_sum_topic_word_count = g.toPythonList(g._sum_topic_word_count),
            py_doc_topic_count = g.toPythonList(g.doc_topic_count), py_topics_convergence = g.toPythonList(g._topics_convergence),
            py_multi_pros = g.toPythonList(g.multi_pros), py_topic_word_count = g.toPythonListOfList(g._topic_word_count),
            py_occurence_to_index_count = g.toPythonListOfList(g.occurence_to_index_count),
            py_corpus = g.toPythonListOfList(g.corpus);

        py::dict py_word_to_id_vocabulary = g.toPythonDict(g.word_to_id_vocabulary),
            py_id_to_word_vocabulary = g.toPythonDict(g._id_to_word_vocabulary);

        return py::make_tuple(g.num_documents, g.num_of_words_in_corpus, g.vocabulary_size, g.alpha_sum, g.beta_sum,
            py_topic_assignments, py_sum_topic_word_count, py_doc_topic_count, py_topics_convergence, py_multi_pros,
            py_topic_word_count, py_occurence_to_index_count, py_corpus, py_word_to_id_vocabulary, py_id_to_word_vocabulary);
    }

    static void setstate(GibbsSamplingDMM& g, py::tuple state) {
        g.num_documents = py::extract<int>(state[0]);
        g.num_of_words_in_corpus = py::extract<int>(state[1]);
        g.vocabulary_size = py::extract<int>(state[2]);
        g.alpha_sum = py::extract<double>(state[3]);
        g.beta_sum = py::extract<double>(state[4]);
        g._topic_assignments = g.toStdVectorInt(state[5]);
        g._sum_topic_word_count = g.toStdVectorInt(state[6]);
        g.doc_topic_count = g.toStdVectorInt(state[7]);
        g._topics_convergence = g.toStdVectorInt(state[8]);
        g.multi_pros = g.toStdVectorDouble(state[9]);
        g._topic_word_count = g.toStdVectorOfVectorInt(state[10]);
        g.occurence_to_index_count = g.toStdVectorOfVectorInt(state[11]);
        g.corpus = g.toStdVectorOfVectorInt(state[12]);
        g.word_to_id_vocabulary = g.toMapStrInt(state[13]);
        g._id_to_word_vocabulary = g.toMapIntStr(state[14]);
    }
};


#pragma once

#include <vector>
#include <Eigen/Dense>

namespace cilantro {
    template <typename ScalarT, ptrdiff_t EigenDim>
    class DataMatrixMap : public Eigen::Map<Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef ScalarT Scalar;

        enum { Dimension = EigenDim };

        DataMatrixMap(Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic> &data)
                : Eigen::Map<Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>(data.data(), data.rows(), data.cols())
        {}

        DataMatrixMap(Eigen::Map<Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>> data)
                : Eigen::Map<Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>(data.data(), data.rows(), data.cols())
        {}

        template <ptrdiff_t Dim = EigenDim, class = typename std::enable_if<Dim != Eigen::Dynamic>::type>
        DataMatrixMap(std::vector<ScalarT> &data)
                : Eigen::Map<Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>((ScalarT *)data.data(), EigenDim, data.size()/EigenDim)
        {}

        template <ptrdiff_t Dim = EigenDim, class = typename std::enable_if<Dim == Eigen::Dynamic>::type>
        DataMatrixMap(std::vector<ScalarT> &data, size_t dim)
                : Eigen::Map<Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>((ScalarT *)data.data(), dim, data.size()/dim)
        {}

        template <ptrdiff_t Dim = EigenDim, class = typename std::enable_if<Dim != Eigen::Dynamic && sizeof(Eigen::Matrix<ScalarT,Dim,1>) % 16 != 0>::type>
        DataMatrixMap(std::vector<Eigen::Matrix<ScalarT,EigenDim,1>> &data)
                : Eigen::Map<Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>((ScalarT *)data.data(), EigenDim, data.size())
        {}

        template <ptrdiff_t Dim = EigenDim, class = typename std::enable_if<Dim != Eigen::Dynamic>::type>
        DataMatrixMap(std::vector<Eigen::Matrix<ScalarT,EigenDim,1>,Eigen::aligned_allocator<Eigen::Matrix<ScalarT,EigenDim,1>>> &data)
                : Eigen::Map<Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>((ScalarT *)data.data(), EigenDim, data.size())
        {}

        template <ptrdiff_t Dim = EigenDim, class = typename std::enable_if<Dim != Eigen::Dynamic>::type>
        DataMatrixMap(ScalarT * data, size_t num_points = 0)
                : Eigen::Map<Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>(data, EigenDim, num_points)
        {}

        template <ptrdiff_t Dim = EigenDim, class = typename std::enable_if<Dim == Eigen::Dynamic>::type>
        DataMatrixMap(ScalarT * data, size_t dim, size_t num_points = 0)
                : Eigen::Map<Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>(data, dim, num_points)
        {}

        inline Eigen::Map<Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>& eigenMap() {
            return (*(Eigen::Map<Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>> *)this);
        }
    };

    typedef DataMatrixMap<float,2> DataMatrixMap2f;
    typedef DataMatrixMap<double,2> DataMatrixMap2d;
    typedef DataMatrixMap<float,3> DataMatrixMap3f;
    typedef DataMatrixMap<double,3> DataMatrixMap3d;
    typedef DataMatrixMap<float,Eigen::Dynamic> DataMatrixMapXf;
    typedef DataMatrixMap<double,Eigen::Dynamic> DataMatrixMapXd;

    // Read-only Eigen Map (for inputs)
    template <typename ScalarT, ptrdiff_t EigenDim>
    class ConstDataMatrixMap : public Eigen::Map<const Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef ScalarT Scalar;

        enum { Dimension = EigenDim };

        ConstDataMatrixMap(const Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic> &data)
                : Eigen::Map<const Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>(data.data(), data.rows(), data.cols())
        {}

        ConstDataMatrixMap(Eigen::Map<const Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>> data)
                : Eigen::Map<const Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>(data.data(), data.rows(), data.cols())
        {}

        ConstDataMatrixMap(Eigen::Map<Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>> data)
                : Eigen::Map<const Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>(data.data(), data.rows(), data.cols())
        {}

        template <ptrdiff_t Dim = EigenDim, class = typename std::enable_if<Dim != Eigen::Dynamic>::type>
        ConstDataMatrixMap(const std::vector<ScalarT> &data)
                : Eigen::Map<const Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>((const ScalarT *)data.data(), EigenDim, data.size()/EigenDim)
        {}

        template <ptrdiff_t Dim = EigenDim, class = typename std::enable_if<Dim == Eigen::Dynamic>::type>
        ConstDataMatrixMap(const std::vector<ScalarT> &data, size_t dim)
                : Eigen::Map<const Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>((const ScalarT *)data.data(), dim, data.size()/dim)
        {}

        template <ptrdiff_t Dim = EigenDim, class = typename std::enable_if<Dim != Eigen::Dynamic && sizeof(Eigen::Matrix<ScalarT,Dim,1>) % 16 != 0>::type>
        ConstDataMatrixMap(const std::vector<Eigen::Matrix<ScalarT,EigenDim,1>,Eigen::aligned_allocator<Eigen::Matrix<ScalarT,EigenDim,1>>> &data)
                : Eigen::Map<const Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>((const ScalarT *)data.data(), EigenDim, data.size())
        {}

        template <ptrdiff_t Dim = EigenDim, class = typename std::enable_if<Dim != Eigen::Dynamic>::type>
        ConstDataMatrixMap(const std::vector<Eigen::Matrix<ScalarT,EigenDim,1>> &data)
                : Eigen::Map<const Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>((const ScalarT *)data.data(), EigenDim, data.size())
        {}

        template <ptrdiff_t Dim = EigenDim, class = typename std::enable_if<Dim != Eigen::Dynamic>::type>
        ConstDataMatrixMap(const ScalarT * data, size_t num_points = 0)
                : Eigen::Map<const Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>(data, EigenDim, num_points)
        {}

        template <ptrdiff_t Dim = EigenDim, class = typename std::enable_if<Dim == Eigen::Dynamic>::type>
        ConstDataMatrixMap(const ScalarT * data, size_t dim, size_t num_points = 0)
                : Eigen::Map<const Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>(data, dim, num_points)
        {}

        inline Eigen::Map<const Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>>& eigenMap() {
            return (*(Eigen::Map<const Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>> *)this);
        }
    };

    typedef ConstDataMatrixMap<float,2> ConstDataMatrixMap2f;
    typedef ConstDataMatrixMap<double,2> ConstDataMatrixMap2d;
    typedef ConstDataMatrixMap<float,3> ConstDataMatrixMap3f;
    typedef ConstDataMatrixMap<double,3> ConstDataMatrixMap3d;
    typedef ConstDataMatrixMap<float,Eigen::Dynamic> ConstDataMatrixMapXf;
    typedef ConstDataMatrixMap<double,Eigen::Dynamic> ConstDataMatrixMapXd;

    template <typename ScalarT, ptrdiff_t EigenDim>
    using ConstVectorSetMatrixMap = ConstDataMatrixMap<ScalarT,EigenDim>;

    typedef ConstVectorSetMatrixMap<float,2> ConstVectorSetMatrixMap2f;
    typedef ConstVectorSetMatrixMap<double,2> ConstVectorSetMatrixMap2d;
    typedef ConstVectorSetMatrixMap<float,3> ConstVectorSetMatrixMap3f;
    typedef ConstVectorSetMatrixMap<double,3> ConstVectorSetMatrixMap3d;
    typedef ConstVectorSetMatrixMap<float,Eigen::Dynamic> ConstVectorSetMatrixMapXf;
    typedef ConstVectorSetMatrixMap<double,Eigen::Dynamic> ConstVectorSetMatrixMapXd;

    template <typename ScalarT, ptrdiff_t EigenDim>
    using ConstHomogeneousVectorSetMatrixMap = typename std::conditional<EigenDim == Eigen::Dynamic, ConstDataMatrixMap<ScalarT,EigenDim>, ConstDataMatrixMap<ScalarT,EigenDim+1>>::type;

    typedef ConstHomogeneousVectorSetMatrixMap<float,2> ConstHomogeneousVectorSetMatrixMap2f;
    typedef ConstHomogeneousVectorSetMatrixMap<double,2> ConstHomogeneousVectorSetMatrixMap2d;
    typedef ConstHomogeneousVectorSetMatrixMap<float,3> ConstHomogeneousVectorSetMatrixMap3f;
    typedef ConstHomogeneousVectorSetMatrixMap<double,3> ConstHomogeneousVectorSetMatrixMap3d;
    typedef ConstHomogeneousVectorSetMatrixMap<float,Eigen::Dynamic> ConstHomogeneousVectorSetMatrixMapXf;
    typedef ConstHomogeneousVectorSetMatrixMap<double,Eigen::Dynamic> ConstHomogeneousVectorSetMatrixMapXd;

    template <typename ScalarT, ptrdiff_t EigenDim>
    using Vector = Eigen::Matrix<ScalarT,EigenDim,1>;

    typedef Vector<float,2> Vector2f;
    typedef Vector<double,2> Vector2d;
    typedef Vector<float,3> Vector3f;
    typedef Vector<double,3> Vector3d;
    typedef Vector<float,Eigen::Dynamic> VectorXf;
    typedef Vector<double,Eigen::Dynamic> VectorXd;

    template <typename ScalarT, ptrdiff_t EigenDim>
    using VectorSet = Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>;

    typedef VectorSet<float,2> VectorSet2f;
    typedef VectorSet<double,2> VectorSet2d;
    typedef VectorSet<float,3> VectorSet3f;
    typedef VectorSet<double,3> VectorSet3d;
    typedef VectorSet<float,Eigen::Dynamic> VectorSetXf;
    typedef VectorSet<double,Eigen::Dynamic> VectorSetXd;

    template <typename ScalarT, ptrdiff_t EigenDim>
    using HomogeneousVector = typename std::conditional<EigenDim == Eigen::Dynamic, Eigen::Matrix<ScalarT,EigenDim,1>, Eigen::Matrix<ScalarT,EigenDim+1,1>>::type;

    typedef HomogeneousVector<float,2> HomogeneousVector2f;
    typedef HomogeneousVector<double,2> HomogeneousVector2d;
    typedef HomogeneousVector<float,3> HomogeneousVector3f;
    typedef HomogeneousVector<double,3> HomogeneousVector3d;
    typedef HomogeneousVector<float,Eigen::Dynamic> HomogeneousVectorXf;
    typedef HomogeneousVector<double,Eigen::Dynamic> HomogeneousVectorXd;

    template <typename ScalarT, ptrdiff_t EigenDim>
    using HomogeneousVectorSet = typename std::conditional<EigenDim == Eigen::Dynamic, Eigen::Matrix<ScalarT,EigenDim,Eigen::Dynamic>, Eigen::Matrix<ScalarT,EigenDim+1,Eigen::Dynamic>>::type;

    typedef HomogeneousVectorSet<float,2> HomogeneousVectorSet2f;
    typedef HomogeneousVectorSet<double,2> HomogeneousVectorSet2d;
    typedef HomogeneousVectorSet<float,3> HomogeneousVectorSet3f;
    typedef HomogeneousVectorSet<double,3> HomogeneousVectorSet3d;
    typedef HomogeneousVectorSet<float,Eigen::Dynamic> HomogeneousVectorSetXf;
    typedef HomogeneousVectorSet<double,Eigen::Dynamic> HomogeneousVectorSetXd;

    enum struct DataMatrixViewMode { Matrix, MatrixRef, ConstMatrixRef, MatrixMap, ConstMatrixMap };

    template <typename ScalarT, ptrdiff_t EigenDim, DataMatrixViewMode Mode>
    struct DataMatrixView {};

    template <typename ScalarT, ptrdiff_t EigenDim>
    struct DataMatrixView<ScalarT,EigenDim,DataMatrixViewMode::Matrix> { typedef VectorSet<ScalarT,EigenDim> Container; };

    template <typename ScalarT, ptrdiff_t EigenDim>
    struct DataMatrixView<ScalarT,EigenDim,DataMatrixViewMode::MatrixRef> { typedef VectorSet<ScalarT,EigenDim>& Container; };

    template <typename ScalarT, ptrdiff_t EigenDim>
    struct DataMatrixView<ScalarT,EigenDim,DataMatrixViewMode::ConstMatrixRef> { typedef const VectorSet<ScalarT,EigenDim>& Container; };

    template <typename ScalarT, ptrdiff_t EigenDim>
    struct DataMatrixView<ScalarT,EigenDim,DataMatrixViewMode::MatrixMap> { typedef DataMatrixMap<ScalarT,EigenDim> Container; };

    template <typename ScalarT, ptrdiff_t EigenDim>
    struct DataMatrixView<ScalarT,EigenDim,DataMatrixViewMode::ConstMatrixMap> { typedef ConstDataMatrixMap<ScalarT,EigenDim> Container; };
}


#ifndef KMCL_PARTICLE_HPP_
#define KMCL_PARTICLE_HPP_
#include <cmath>

#include <Eigen/Dense>
#include <Eigen/StdVector>
#include "rng.hpp"
// sandymfallon:
//#include <math_utils/math_utils.hpp>
#include <pronto_utils/pronto_math.hpp>

//NaN is the only value, for which is expression value == value always false.
template<typename T>
inline bool isnan_particle(T value)
{
return value != value;
}

// State Vector
class pf_state{
public:
  Eigen::Isometry3d pose;
  Eigen::Isometry3d velocity;
};

///////////////////////////////////////////////////////////////
// A particle
class Particle{
    public:
      Particle(){}

      ~Particle(){};

      void Set(pf_state state_in,double logweight_in){
        state = state_in;
        logweight = logweight_in;
      }

      void InitializeState(rng *pRng, double init_weight,
          Eigen::Isometry3d init_pose, std::vector<double> initial_var);

      void SetState(const pf_state & sValue){ state = sValue;  }

      pf_state GetState(){ return state; }

      /// Returns the particle's log weight.
      double GetLogWeight(){ return logweight; }
      /// Returns the particle's unnormalised weight.
      double GetWeight(void) const {return exp(logweight);}

      void MoveParticleDrift(rng *pRng, std::vector<double> move_var,double dtime);

      void MoveParticle(rng *pRng, pf_state odom_diff, std::vector<double> move_var,double dtime);

      // [Disabled - assume Lhood specified using AddToLogWeight]
      //double LogLikelihood(){//double x_meas,double y_meas){
      //return 1;
      //}

      void AddToLogWeight(double dIncrement) { logweight += dIncrement;}
      void SetLogWeight(const double & dLogWeight) {logweight = dLogWeight;}

  private:
      pf_state state;
      /// Natural logarithm of this particle's weight.
      double logweight;
};

#endif /* CONFIGURATION_HPP_ */


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include <boost/python/copy_const_reference.hpp>

#include <scitbx/math/zernike.h>

namespace scitbx { namespace math {

namespace {

  struct nlm_array_wrapper
  {
    typedef zernike::nlm_array<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("nlm_array", no_init)
        .def(init<int const&>
                  ((arg("n_max") )))
        .def("set_coef", &w_t::set_coef )
        .def("get_coef", &w_t::get_coef )
        .def("load_coefs", &w_t::load_coefs)
        .def("select_on_nl", &w_t::select_on_nl)
        .def("nlm", &w_t::nlm)
        .def("coefs", &w_t::coefs)
       ;
    }
  };

  struct nmk_array_wrapper
  {
    typedef zernike::nmk_array<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("nmk_array", no_init)
        .def(init<int const&>
                  ((arg("n_max") )))
        .def("set_coef", &w_t::set_coef )
        .def("get_coef", &w_t::get_coef )
        .def("load_coefs", &w_t::load_coefs)
        .def("select_on_nl", &w_t::select_on_nl)
        .def("nlm", &w_t::nlm)
        .def("coefs", &w_t::coefs)
       ;
    }
  };

  struct nl_array_wrapper
  {
    typedef zernike::nl_array<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("nl_array", no_init)
        .def(init<int const&>
                  ((arg("n_max") )))
        .def("set_coef", &w_t::set_coef )
        .def("get_coef", &w_t::get_coef )
        .def("load_coefs", &w_t::load_coefs)
        .def("nl", &w_t::nl)
        .def("coefs", &w_t::coefs)
       ;
    }
  };

  struct nl_complex_array_wrapper
  {
    typedef zernike::nl_complex_array< double > w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("nl_c_array", no_init)
        .def(init<int const&>
                  ((arg("n_max") )))
        .def("set_coef", &w_t::set_coef )
        .def("get_coef", &w_t::get_coef )
        .def("load_coefs", &w_t::load_coefs)
        .def("nl", &w_t::nl)
        .def("coefs", &w_t::coefs)
       ;
    }
  };

  struct log_factorial_generator_wrapper
  {
    typedef zernike::log_factorial_generator<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("log_factorial_generator", no_init)
        .def(init<int const&>
                  ((arg("n_max") )))
        .def("log_fact", &w_t::log_fact )
        .def("fact", &w_t::fact )
       ;
    }
  };

  struct nss_spherical_harmonics_wrapper
  {
    typedef zernike::nss_spherical_harmonics<> w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      // int const& max_l, int const& mangle, log_factorial_generator<FloatType> const& lgf
      class_<w_t>("nss_spherical_harmonics", no_init)
        .def(init<int const&, int const&, zernike::log_factorial_generator<double> const&>
                  ((arg("l_max"), arg("mangle"), arg("lgf") )))
        .def("legendre_lm", &w_t::legendre_lm)
        .def("legendre_lm_pc", &w_t::legendre_lm_pc)
        .def("spherical_harmonic_pc", &w_t::spherical_harmonic_pc)
        .def("spherical_harmonic", &w_t::spherical_harmonic_direct)
       ;
    }
  };

  struct zernike_radial_wrapper
  {
    typedef zernike::zernike_radial<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("zernike_radial", no_init)
        .def(init<int const& , int const&, zernike::log_factorial_generator<double> const&>
                  ((arg("n"),
                    arg("l"),
                    arg("log_factorial_generator")
                   )) )
        .def("Nnlk", &w_t::Nnlk)
        .def("f", (double(w_t::*)(double const&)) &w_t::f)
        .def("f", (scitbx::af::shared<double>(w_t::*)
                   (scitbx::af::const_ref<double> const&))
                    &w_t::f )
       ;
    }
  };

  struct zernike_2d_radial_wrapper
  {
    typedef zernike::zernike_2d_radial<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("zernike_2d_radial", no_init)
        .def(init<int const& , int const&, zernike::log_factorial_generator<double> const&>
                  ((arg("n"),
                    arg("l"),
                    arg("log_factorial_generator")
                   )) )
        .def("Nnlk", &w_t::Nnlk)
        .def("f", (double(w_t::*)(double const&)) &w_t::f)
        .def("f", (scitbx::af::shared<double>(w_t::*)
                   (scitbx::af::const_ref<double> const&))
                    &w_t::f )
       ;
    }
  };

  struct zernike_polynome_wrapper
  {
    typedef zernike::zernike_polynome<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("zernike_polynome", no_init)
        .def(init<int const&, int const&, int const&, zernike::zernike_radial<double> const& >
                  ((arg("n"),
                    arg("l"),
                    arg("m"),
                    arg("Rnl")
                   )))
         .def("f", &w_t::f)
       ;
    }
  };

  struct zernike_2d_polynome_wrapper
  {
    typedef zernike::zernike_2d_polynome<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("zernike_2d_polynome", no_init)
        .def(init<int const&, int const&> //, zernike::zernike_2d_radial_dc<double> const& >
                  ((arg("n"),
                    arg("l")
//                    arg("Rnl")
                   )))
         .def("f", &w_t::f)
       ;
    }
  };

  struct zernike_grid_wrapper
  {
    typedef zernike::zernike_grid<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("zernike_grid", no_init)
        .def(init<int const&, int const&, bool const&>
                  ((  arg("m"),
                      arg("n_max"),
                      arg("hex")
                  )))
        .def("xyz", &w_t::xyz )
        .def("rtp", &w_t::rtp )
        //.def("get_coef", &w_t::get_coef )
        .def("load_coefs", &w_t::load_coefs)
        .def("f", &w_t::f)
        .def("f_real", &w_t::f_real)
        .def("nlm", &w_t::nlm)
        .def("coefs", &w_t::coefs)
        .def("slow_moments", &w_t::slow_moments)
       ;
    }
  };

  struct zernike_grid_2d_wrapper
  {
    typedef zernike::zernike_grid_2d<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("zernike_grid_2d", no_init)
        .def(init<int const&, int const&>
                  ((  arg("m"),
                      arg("n_max")
                  )))
        .def("xy", &w_t::xy )
        .def("rt", &w_t::rt )
        .def("load_coefs", &w_t::load_coefs)
        .def("f", &w_t::f)
        .def("nl", &w_t::nl)
        .def("coefs", &w_t::coefs)
           ;
    }
  };

} // namespace <anonymous>

namespace boost_python {

  void wrap_zernike()
  {
    nlm_array_wrapper::wrap();
    nmk_array_wrapper::wrap();
    nl_array_wrapper::wrap();
    nl_complex_array_wrapper::wrap();
    zernike_polynome_wrapper::wrap();
    zernike_radial_wrapper::wrap();
    zernike_2d_radial_wrapper::wrap();
    zernike_2d_polynome_wrapper::wrap();
    log_factorial_generator_wrapper::wrap();
    zernike_grid_wrapper::wrap();
    zernike_grid_2d_wrapper::wrap();
    nss_spherical_harmonics_wrapper::wrap();
  }

}}} // namespace scitbx::math::boost_python


// kate: replace-tabs off; indent-width 4; indent-mode normal
// vim: ts=4:sw=4:noexpandtab
/*

Copyright (c) 2010--2018,
François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
You can contact the authors at <f dot pomerleau at gmail dot com> and
<stephane at magnenat dot net>

All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the <organization> nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/
#include "RemoveSensorBias.h"

#include "PointMatcherPrivate.h"

#include <string>
#include <vector>

#include <boost/format.hpp>
#include <ciso646>
#include <cmath>

template<typename T>
RemoveSensorBiasDataPointsFilter<T>::RemoveSensorBiasDataPointsFilter(const Parameters& params):
	PointMatcher<T>::DataPointsFilter("RemoveSensorBiasDataPointsFilter",
		RemoveSensorBiasDataPointsFilter::availableParameters(), params),
	sensorType(SensorType(Parametrizable::get<int>("sensorType"))),
	angleThreshold(Parametrizable::get<T>("angleThreshold")/180.*M_PI)
{
}

template<typename T>
typename PointMatcher<T>::DataPoints
RemoveSensorBiasDataPointsFilter<T>::filter(const DataPoints& input)
{
	DataPoints output(input);
	inPlaceFilter(output);
	return output;
}

template<typename T>
void RemoveSensorBiasDataPointsFilter<T>::inPlaceFilter(DataPoints& cloud)
{
	//Check if there is normals info
	if (!cloud.descriptorExists("incidenceAngles"))
		throw InvalidField("RemoveSensorBiasDataPointsFilter: Error, cannot find incidence angles in descriptors.");
	//Check if there is normals info
	if (!cloud.descriptorExists("observationDirections"))
		throw InvalidField("RemoveSensorBiasDataPointsFilter: Error, cannot find observationDirections in descriptors.");

	const auto& incidenceAngles = cloud.getDescriptorViewByName("incidenceAngles");
	const auto& observationDirections = cloud.getDescriptorViewByName("observationDirections");

	double aperture, k1, k2;

	switch(sensorType)
	{
		case LMS_1XX:
		{
			aperture = SensorParameters::LMS_1XX.aperture;
			k1 = SensorParameters::LMS_1XX.k1;
			k2 = SensorParameters::LMS_1XX.k2;
			break;
		}
		case HDL_32E:
		{
			aperture = SensorParameters::HDL_32E.aperture;
			k1 = SensorParameters::HDL_32E.k1;
			k2 = SensorParameters::HDL_32E.k2;
			break;
		}
		default:
			throw InvalidParameter(
				(boost::format("RemoveSensorBiasDataPointsFilter: Error, cannot remove bias for sensorType id %1% .") % sensorType).str());
	}

	const std::size_t nbPts = cloud.getNbPoints();
	const std::size_t dim = cloud.features.rows();

	assert(dim == 3 or dim == 4); //check 2D or 3D

	std::size_t j = 0;
	for(std::size_t i = 0; i < nbPts; ++i)
	{
		const Vector vObs = observationDirections.col(i);

		const double depth = vObs.norm();
		const T incidence = incidenceAngles(0, i);

		//check if the incidence angle could be estimated.
		//For angles very close to 90 degrees, a small error of estimation could change drastically the correction, so we skip those points.
		if(not std::isnan(incidence) and incidence >= 0. and incidence < angleThreshold)
		{
			const double correction = k1 * diffDist(depth, incidence, aperture) + k2 * ratioCurvature(depth, incidence, aperture);

			Vector p = cloud.features.col(i);
			p.head(dim-1) += correction * vObs.normalized();
			cloud.features.col(i) = p;
			cloud.setColFrom(j, cloud, i);
			++j;
		}
	}
	cloud.conservativeResize(j);
}

template<typename T>
std::array<double, 4> RemoveSensorBiasDataPointsFilter<T>::getCoefficients(const double depth, const T theta, const double aperture) const
{
	const double sigma = tau / std::sqrt(2. * M_PI);
	const double w0 = lambda_light / (M_PI * aperture);

	const double A  = 2. * std::pow(depth * std::tan(theta), 2) / std::pow(sigma * c, 2) + 2. / std::pow(aperture, 2);
	const double K1 = std::pow(std::cos(theta), 3);
	const double K2 = 3. * std::pow(std::cos(theta), 2) * std::sin(theta);
	const double L1 = pulse_intensity * std::pow(w0 / (aperture * depth * std::cos(theta)), 2) *
		std::sqrt(M_PI) * std::erf(aperture * std::sqrt(A)) / (2. * std::pow(A, 3. / 2.));
	const double L2 = pulse_intensity * std::pow(w0 / (aperture * depth * std::cos(theta)), 2) * K2 / (2. * A);

	const double a0 = 2. * A * K1 * L1;
	const double a1 = -(2. * std::tan(theta) * depth *
			(L1 * K2 - 2. * L2 * aperture * std::exp(-A * std::pow(aperture, 2)))) / (std::pow(sigma, 2) * c);
	const double a2 = -L1 * 2. * A * K1 * (std::pow(sigma * c * std::cos(theta), 2) * A + 2. * std::pow(std::cos(theta) * depth, 2) - 2. * std::pow(depth, 2)) /
		(2 * std::pow(c * std::cos(theta), 2) * std::pow(sigma, 4) * A);
	const double a3 = L1 * K2 * depth * std::tan(theta) * (std::pow(sigma * c, 2) * A - 2. * std::pow(depth * std::tan(theta), 2)) /
		(std::pow(sigma, 6) * std::pow(c, 3) * A);

	return {a0, a1, a2, a3};
}

template<typename T>
double RemoveSensorBiasDataPointsFilter<T>::diffDist(const double depth, const T theta, const double aperture) const
{
	static constexpr double epsilon = 1e-5;

	const std::array<double, 4> a = getCoefficients(depth, theta, aperture);

	double Tmax;

	if(theta < epsilon) // approx. 0
		Tmax = 0.;
	else // theta >0
		Tmax = (-2 * a[2] - std::sqrt(4 * pow(a[2], 2) - 12 * a[1] * a[3])) / (6 * a[3]);

	return Tmax * c / 2.;
}

template<typename T>
double RemoveSensorBiasDataPointsFilter<T>::ratioCurvature(const double depth, const T theta, const double aperture) const
{
	static constexpr double epsilon = 1e-5;
	const std::array<double,4> a = getCoefficients(depth, theta, aperture);
	const std::array<double,4> b = getCoefficients(depth, 0., aperture);

	double Tmax;

	if(theta < epsilon) // approx. 0
		Tmax = 0.;
	else // theta >0
		Tmax = ( -2 * a[2] - std::sqrt(4 * std::pow(a[2], 2) - 12 * a[1] * a[3])) / (6 * a[3]);

	return 1. - 2 * b[2] / (2 * a[2] + 6 * Tmax * a[3]);
}

template struct RemoveSensorBiasDataPointsFilter<float>;
template struct RemoveSensorBiasDataPointsFilter<double>;


// Copyright 2014 Josh Pieper, jjp@pobox.com.
//
// 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 "base/ukf_filter.h"

#include <boost/test/auto_unit_test.hpp>

namespace {
typedef mjmech::base::UkfFilter<double, 3> UkfFilter;
}

BOOST_AUTO_TEST_CASE(BasicUkfFilter) {
  auto test_process = [](const UkfFilter::State& s,
                         double dt_s) -> UkfFilter::State {
    UkfFilter::State delta;
    delta(0, 0) = 0.;
    delta(1, 0) = s(0) * dt_s;
    delta(2, 0) = s(1) * dt_s + 0.5 * s(0) * dt_s * dt_s;
    return s + delta;
  };

  auto test_measurement = [](const UkfFilter::State& s) {
    Eigen::Matrix<double, 1, 1> r;
    r(0, 0) = s(2);
    return r;
  };

  UkfFilter::Covariance cov = UkfFilter::Covariance::Zero();
  cov(0, 0) = 1.0;
  cov(1, 1) = 2.0;
  cov(2, 2) = 3.0;

  UkfFilter::Covariance proc = UkfFilter::Covariance::Zero();
  proc(0, 0) = 0.1;
  proc(1, 1) = 0.1;
  proc(2, 2) = 0.1;
  UkfFilter dut(UkfFilter::State(0.2, 0.0, 0.0),
                cov,
                proc);

  Eigen::Matrix<double, 1, 1> meas;
  meas(0, 0) = 0.5;
  Eigen::Matrix<double, 1, 1> meas_noise;
  meas_noise(0, 0) = 2.0;

  for (int i = 0; i < 200; i++) {
    meas(0, 0) += 0.5;
    dut.UpdateState(0.1, test_process);
    dut.UpdateMeasurement(test_measurement, meas, meas_noise);
  }

  BOOST_CHECK_SMALL(dut.state()(2) - meas(0), 1e-2);
  BOOST_CHECK_SMALL(dut.state()(1) - 0.5 / 0.1, 1e-2);
  BOOST_CHECK(dut.covariance()(0, 0) > 0.0);
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_ABS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_ABS_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/abs.hpp>
#include <math.h>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::abs_, tag::cpu_
                            , (A0)
                            , (scalar_< signed_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return (a0 >= 0)?a0:-a0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::abs_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return  (a0 >= 0)?a0:-a0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::abs_, tag::cpu_
                            , (A0)
                            , (scalar_< int32_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      A0 mask = a0 >> (sizeof(int32_t)*8 - 1);
      return (a0 + mask) ^ mask;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::abs_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
       return ::fabs(a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::abs_, tag::cpu_
                            , (A0)
                            , (scalar_< unsigned_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return a0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::abs_, tag::cpu_
                            , (A0)
                            , (scalar_< bool_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return a0;
    }
  };
} } }

#endif


/* The Computer Language Benchmarks Game
 * http://benchmarksgame.alioth.debian.org/
 *
 * contributed by Jon Harrop
 * modified by Alex Mizrahi
 * modified by Andreas Schäfer
 * very minor omp tweak by The Anh Tran
 * modified by Maxime Coste
 */

#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <omp.h>

#include <boost/pool/object_pool.hpp>

const size_t   LINE_SIZE = 64;

struct Node
{
    Node *l, *r;
    int i;

    int check() const { return l ? l->check() + i - r->check() : i; }
};

typedef boost::object_pool<Node> NodePool;

Node *make(int i, int d, NodePool &store)
{
    return (d == 0) ? nullptr : store.construct(Node{make(2*i-1, d-1, store), make(2*i, d-1, store), i});
}

int GetThreadCount()
{
   cpu_set_t cs;
   CPU_ZERO(&cs);
   sched_getaffinity(0, sizeof(cs), &cs);

   int count = 0;
   for (int i = 0; i < 8; i++)
   {
      if (CPU_ISSET(i, &cs))
         count++;
   }
   return count;
}

int run(int input)
{
    int min_depth = 4;
    int max_depth = std::max(min_depth+2, input);
    int stretch_depth = max_depth+1;

   // Alloc then dealloc stretchdepth tree
    {
        NodePool store;
        Node *c = make(0, stretch_depth, store);
        std::cout << "stretch tree of depth " << stretch_depth << "\t "
                  << "check: " << c->check() << std::endl;
    }

    NodePool long_lived_store;
    Node *long_lived_tree = make(0, max_depth, long_lived_store);

   // buffer to store output of each thread
   char *outputstr = (char*)malloc(LINE_SIZE * (max_depth +1) * sizeof(char));

   #pragma omp parallel for default(shared) num_threads(GetThreadCount()) schedule(dynamic, 1)
    for (int d = min_depth; d <= max_depth; d += 2)
    {
        int iterations = 1 << (max_depth - d + min_depth);
        int c = 0;

        for (int i = 1; i <= iterations; ++i)
        {
            NodePool store;
            Node *a = make(i, d, store), *b = make(-i, d, store);
            c += a->check() + b->check();
        }

      // each thread write to separate location
      sprintf(outputstr + LINE_SIZE * d, "%d\t trees of depth %d\t check: %d\n", (2 * iterations), d, c);
   }

   // print all results
   for (int d = min_depth; d <= max_depth; d += 2)
      printf("%s", outputstr + (d * LINE_SIZE) );
   free(outputstr);

    std::cout << "long lived tree of depth " << max_depth << "\t "
             << "check: " << (long_lived_tree->check()) << "\n";

    return 0;
}

#include "nbind/nbind.h"

NBIND_GLOBAL() {
  function(run);
}

/*

NOTES:
64-bit Ubuntu quad core
g++ (Ubuntu 5.4.0-6ubuntu1~16.04.1) 5.4.0 20160609

Wed, 27 Apr 2016 18:02:26 GMT

MAKE:
/usr/bin/g++ -c -pipe -O3 -fomit-frame-pointer -march=native  -std=c++14 binarytrees.gpp-7.c++ -o binarytrees.gpp-7.c++.o &&  \
        /usr/bin/g++ binarytrees.gpp-7.c++.o -o binarytrees.gpp-7.gpp_run -fopenmp -lboost_system
rm binarytrees.gpp-7.c++
8.37s to complete and log all make actions

COMMAND LINE:
./binarytrees.gpp-7.gpp_run 20

PROGRAM OUTPUT:
stretch tree of depth 21   check: -1
2097152  trees of depth 4  check: -2097152
524288   trees of depth 6  check: -524288
131072   trees of depth 8  check: -131072
32768  trees of depth 10   check: -32768
8192   trees of depth 12   check: -8192
2048   trees of depth 14   check: -2048
512  trees of depth 16   check: -512
128  trees of depth 18   check: -128
32   trees of depth 20   check: -32
long lived tree of depth 20  check: -1

*/


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "ComplexArrayExtensions.h"
#include "VectorFromMatrixSliceIterator.h"

#include "MathExtensions.h"
#include "Random.h"

#define BOOST_TEST_MODULE BlitzExtensions test
#include <boost/test/unit_test.hpp>

#include <boost/range/combine.hpp>

using namespace std;
using namespace blitzplusplus;
using namespace linalg;
using namespace cppqedutils;
using randomutils::fill;

using URD=std::uniform_real_distribution<double>;

const double epsilonCmp=1e-12;

void myFunc(const CArray<6>& aa, const CArray<3>& vv, CArray<3>& ww)
{
  using namespace blitz::tensor;
  CArray<6> temp(aa.shape());
  temp=aa(i,j,k,l,m,n)*vv(l,m,n);
  ww=sum(sum(sum(temp,n),m),l);
}

const int RA=4;

typedef CArray<  RA> CAR ;
typedef CArray<2*RA> CA2R;

typedef CArray<RA-1> CARM1;
typedef CArray<RA+1> CARP1;

std::mt19937 ran{1001};

BOOST_AUTO_TEST_CASE( TwoTimesRealPartOfSelfTest )
{
  ExtTiny<RA/2> dims(6,4);
  CAR array(concatenateTinies(dims,dims));
  fill<URD>(array,ran);

  CAR arrayHC(hermitianConjugate(array));
  CAR arrayReal(array.shape()); arrayReal=(array+arrayHC);

  CAR arrayLow(array.copy());
  CMatrix matrixView(binaryArray(arrayLow));
  calculateTwoTimesRealPartOfSelf(matrixView);

  //cout<<array;
  //cout<<array.ordering()<<' '<<array.stride()<<' '<<array.shape()<<endl;
  //cout<<arrayHC.ordering()<<' '<<arrayHC.stride()<<' '<<arrayHC.shape()<<endl;
  //cout<<matrixView.ordering()<<' '<<matrixView.stride()<<' '<<matrixView.shape()<<endl;

  hermitianConjugateSelf(array);

  BOOST_CHECK(all(arrayReal==hermitianConjugate(arrayReal)));
  cout<<"Hermitian conjugation OK\n";
  BOOST_CHECK(all(array==arrayHC));
  cout<<"In-place Hermitian conjugation OK\n";
  BOOST_CHECK(all(arrayLow ==hermitianConjugate(arrayLow )));
  BOOST_CHECK(all(arrayLow ==arrayReal                    ));
  cout<<"In-place real part of matrices OK\n";

}

BOOST_AUTO_TEST_CASE( DoDirectTest )
{
  ExtTiny<RA-1> dims0(4,5,2);
  ExtTiny<RA+1> dims1(3,4,2,4,5);

  CARM1 am1(dims0);
  CARP1 ap1(dims1);

  fill<URD>(ap1,fill<URD>(am1,ran));

  CA2R a2r(concatenateTinies(dims0,dims1));
  {
    using namespace blitz::tensor;
    a2r=am1(i,j,k)*ap1(l,m,n,o,p);
  }

  BOOST_CHECK(all(doDirect<dodirect::multiplication>(am1,ap1)==a2r));
  cout<<"Direct product OK\n";

  {
    using namespace blitz::tensor;
    a2r=am1(i,j,k)+ap1(l,m,n,o,p);
  }

  BOOST_CHECK(all(doDirect<dodirect::addition>(am1,ap1)==a2r));
  cout<<"Direct sum OK\n";
}

BOOST_AUTO_TEST_CASE( MatrixWithVectorMultiplication ) // computing v*a (v acting on only certain indices) in two ways: with tensor and basi
{
  ExtTiny<5> dims1(3,4,2,6,5);
  ExtTiny<3> dims0(dims1(2),dims1(1),dims1(4));

  CArray<6> a(concatenateTinies(dims0,dims0));
  CArray<5> v(dims1), vResTensor(dims1), vResBASI(dims1);

  fill<URD>(v,fill<URD>(a,ran));

  {
    using namespace blitz::tensor;
    CArray<8> temp8(concatenateTinies(
                                      concatenateTinies(
                                                        dims0,
                                                        ExtTiny<2>(dims1(0),dims1(3))
                                                        ),
                                      dims0
                                      ));
    temp8=a(i,j,k,n,o,p)*v(l,o,n,m,p);
    vResTensor=CArray<5>(sum(sum(sum(temp8,p),o),n)).transpose(3,1,0,4,2);
  }

  {
    typedef tmptools::Vector<2,1,4> V214;
    // TODO: in c++20 this should be solvable with something like this:
    // for ( auto [v,w] : {cppqedutils::sliceiterator::fullRange<V214>(v),cppqedutils::sliceiterator::fullRange<V214>(vResBASI)} ) myFunc(a,v,w);
    for (auto&& [v,w] :  boost::combine(cppqedutils::sliceiterator::fullRange<V214>(v),cppqedutils::sliceiterator::fullRange<V214>(vResBASI)))
      myFunc(a,v,w);
  }

  BOOST_CHECK(all(vResBASI==vResTensor));
  cout<<"BlitzArraySliceIterator OK\n";
}

BOOST_AUTO_TEST_CASE( MatrixProducts ) // VFMSI for matrix products a*rho
{
  ExtTiny<3> dims0(4,5,2);
  CArray<6> rho(concatenateTinies(dims0,dims0)), a(rho.shape()), resTensor(rho.shape());

  fill<URD>(rho,fill<URD>(a,ran));

  {
    using namespace blitz::tensor;
    CArray<9> temp9(concatenateTinies(rho.shape(),dims0));
    temp9=a(i,j,k,o,p,q)*rho(o,p,q,l,m,n);
    resTensor=CArray<6>(sum(sum(sum(temp9,q),p),o));
  }

  {
    using namespace blitzplusplus::vfmsi;
    for (auto v : fullRange<Left>(rho)) myFunc(a,v,v);
  }
  //cout<<max(abs(rho-resTensor))<<endl;
  //BOOST_CHECK(!fcmp(1-max(abs(rho-hermitianConjugate(rho))),1,epsilonCmp));
  BOOST_CHECK(all(rho==resTensor));
  cout<<"VectorFromMatrixSliceIterator matrix product OK\n";
}

BOOST_AUTO_TEST_CASE( VFMSI_Test ) // VFMSI computing a*rho*adagger (rho Hermitian)
{
  ExtTiny<3> dims0(4,5,2);
  CArray<6> rho(concatenateTinies(dims0,dims0)), a(rho.shape()), resTensor(rho.shape());
  CMatrix matrixView(binaryArray(rho));

  fill<URD>(rho,fill<URD>(a,ran));

  calculateTwoTimesRealPartOfSelf(matrixView);
  {
    using namespace blitz::tensor;
    CArray<12> temp12(concatenateTinies(rho.shape(),rho.shape()));
    temp12=a(i,j,k,o,p,q)*rho(o,p,q,r,s,t)*conj(a(l,m,n,r,s,t));
    resTensor=CArray<6>(sum(sum(sum(sum(sum(sum(temp12,t),s),r),q),p),o));
  }
  BOOST_CHECK(!fcmp(1-max(abs(resTensor-hermitianConjugate(resTensor))),1,epsilonCmp));
  {
    {
      using namespace blitzplusplus::vfmsi;
      for (auto v : fullRange<Left>(rho)) myFunc(a,v,v);
    }
    hermitianConjugateSelf(rho);
    {
      using namespace blitzplusplus::vfmsi;
      for (auto v : fullRange<Left>(rho)) myFunc(a,v,v);
    }
  }
  //cout<<max(abs(rho-resTensor))<<endl;
  BOOST_CHECK(!fcmp(1-max(abs(rho-hermitianConjugate(rho))),1,epsilonCmp));
  BOOST_CHECK(!fcmp(1-max(abs(rho-resTensor)),1,epsilonCmp));
  cout<<"VectorFromMatrixSliceIterator matrix sandwich OK\n";
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/normstat.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/fusion/include/vector_tie.hpp>

#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL( normstat, NT2_REAL_TYPES)
{
  using nt2::normstat;
  using nt2::tag::normstat_;
  using nt2::sqr;

  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<normstat_(T, T)>::type)
                  , (std::pair<T,T>)
                  );

  T mean;
  T var;
  T m = T(1.5);
  T s = T(2.5);
  {
    normstat(m, s, mean, var);
    NT2_TEST_EQUAL(var, sqr(s));
    NT2_TEST_EQUAL(mean, m);
  }

  {
    mean = normstat(m, s, var);
    NT2_TEST_EQUAL(var, sqr(s));
    NT2_TEST_EQUAL(mean, m);
  }

  {
    boost::fusion::vector_tie(mean,var) = normstat(m, s);
    NT2_TEST_EQUAL(var, sqr(s));
    NT2_TEST_EQUAL(mean, m);
  }

  {
    std::pair<T,T> p;

    p = normstat(m, s);
    NT2_TEST_EQUAL(p.first, m);
    NT2_TEST_EQUAL(p.second, sqr(s));
  }
}


#ifndef PARMCB_SPVECGF2_HPP_
#define PARMCB_SPVECGF2_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <vector>
#include <set>
#include <iostream>
#include <cassert>

#include <boost/serialization/vector.hpp>

namespace parmcb {

    template<typename U>
    class SpVecGF2 {

    public:

        typedef typename std::vector<U>::size_type size_type;
        typedef typename std::vector<U>::const_iterator const_iterator;

        SpVecGF2() {
        }

        SpVecGF2(const U &i) {
            ones.push_back(i);
        }

        SpVecGF2(const SpVecGF2<U> &v) {
            ones = v.ones;
        }

        SpVecGF2(const SpVecGF2<U> &&v) {
            ones = std::move(v.ones);
        }

        SpVecGF2(const std::set<U> &v) {
            std::copy(v.begin(), v.end(), std::back_inserter(ones));
        }

        ~SpVecGF2(void) {
        }

        SpVecGF2<U>& operator=(const SpVecGF2<U> &v) {
            if (this == &v) {
                return *this;
            }
            ones = v.ones;
            return *this;
        }

        SpVecGF2<U>& operator=(const SpVecGF2<U> &&v) {
            if (this == &v) {
                return *this;
            }
            ones = std::move(v.ones);
            return *this;
        }

        int operator*(const SpVecGF2<U> &v) const {
            int res = 0;

            auto it = ones.begin(), it_e = ones.end();
            auto v_it = v.ones.begin(), v_it_e = v.ones.end();

            while (it != it_e && v_it != v_it_e) {
                U index = *it;
                U v_index = *v_it;
                if (index > v_index) {
                    v_it++;
                } else if (index < v_index) {
                    it++;
                } else {
                    res = (res + 1) % 2;
                    it++;
                    v_it++;
                }
            }
            return res;
        }

        int operator*(const std::set<U> &v) const {
            int res = 0;

            auto it = ones.begin(), it_e = ones.end();
            auto v_it = v.begin(), v_it_e = v.end();

            while (it != it_e && v_it != v_it_e) {
                U index = *it;
                U v_index = *v_it;
                if (index > v_index) {
                    v_it++;
                } else if (index < v_index) {
                    it++;
                } else {
                    res = (res + 1) % 2;
                    it++;
                    v_it++;
                }
            }
            return res;
        }

        SpVecGF2<U> operator+(const SpVecGF2<U> &v) const {
            SpVecGF2<U> res;
            auto it = ones.begin(), it_e = ones.end();
            auto v_it = v.ones.begin(), v_it_e = v.ones.end();

            // now add them
            while (it != it_e && v_it != v_it_e) {
                U index = *it;
                U v_index = *v_it;

                if (index > v_index) {
                    res.ones.push_back(v_index);
                    v_it++;
                } else if (index < v_index) {
                    res.ones.push_back(index);
                    it++;
                } else {
                    // 1 + 1 = 0, don't add anything
                    it++;
                    v_it++;
                }
            }

            // append remaining stuff
            while (it != it_e) {
                res.ones.push_back(*it);
                it++;
            }
            while (v_it != v_it_e) {
                res.ones.push_back(*v_it);
                v_it++;
            }

            return res;
        }

        SpVecGF2<U>& operator+=(const SpVecGF2<U> &v) {
            *this = *this + v;
            return *this;
        }

        void add(U pos) {
            assert(ones.empty() || pos >= *(ones.end()));

            ones.push_back(pos);
        }

        size_type size() const {
            return ones.size();
        }

        const_iterator begin() const
        {
            return ones.begin();
        }

        const_iterator end() const
        {
            return ones.end();
        }

        void clear() {
            ones.clear();
        }

    private:
        friend class boost::serialization::access;

        template<class Archive>
        void serialize(Archive & ar, const unsigned int version)
        {
          ar & ones;
        }

        std::vector<U> ones;
    };

    template<typename U>
    std::ostream& operator<<(std::ostream &o, const SpVecGF2<U> &v) {
        if (v.size() > 0)
            o << "(" << v.size() << ") ";
        std::copy(v.begin(), v.end(), std::ostream_iterator<U> { o, " " });
        return o;
    }

} // parmcb

#endif


// Array expression benchmark

#include <blitz/array.h>
#include <blitz/benchext.h>

BZ_USING_NAMESPACE(blitz)

void blitzVersion(BenchmarkExt<int>& bench);
void CVersion(BenchmarkExt<int>& bench);

int main()
{
    BenchmarkExt<int> bench("Array expression", 2);

    bench.beginBenchmarking();
    blitzVersion(bench);
    CVersion(bench);
    bench.endBenchmarking();

    bench.saveMatlabGraph("arrexpr1.m");

    return 0;
}

void blitzVersion(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Blitz++");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Blitz++: N = " << N << endl;

        long iters = bench.getIterations();

        Array<double,1> x(N);

        // Tickle
        x = 0.;
        firstIndex i;

        bench.start();
        for (long it=0; it < iters; ++it)
        {
            x = i * i;
        }
        bench.stop();
    }

    bench.endImplementation();
}

void CVersion(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("C");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "C: N = " << N << endl;

        long iters = bench.getIterations();

        double* x = new double[N];

        // Tickle
        for (int i=0; i < N; ++i)
            x[i] = 0;

        bench.start();
        for (long it=0; it < iters; ++it)
        {
            for (int i=0; i < N; ++i)
                x[i] = i * i;
        }
        bench.stop();

        delete [] x;
    }

    bench.endImplementation();
}


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

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

 * See LICENSE for the license information

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

/**
 * @file    testSymbolicBayesNetB.cpp
 * @brief   Unit tests for a symbolic Bayes chain
 * @author  Frank Dellaert
 */

#include <boost/assign/list_inserter.hpp> // for 'insert()'
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;

#include <CppUnitLite/TestHarness.h>

#include <gtsam/base/Testable.h>
#include <tests/smallExample.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>

using namespace std;
using namespace gtsam;
using namespace example;

using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* */
TEST( SymbolicBayesNet, constructor )
{
  Ordering o; o += X(2),L(1),X(1);
	// Create manually
	IndexConditional::shared_ptr
		x2(new IndexConditional(o[X(2)],o[L(1)], o[X(1)])),
		l1(new IndexConditional(o[L(1)],o[X(1)])),
		x1(new IndexConditional(o[X(1)]));
	BayesNet<IndexConditional> expected;
	expected.push_back(x2);
	expected.push_back(l1);
	expected.push_back(x1);

	// Create from a factor graph
	GaussianFactorGraph factorGraph = createGaussianFactorGraph(o);
	SymbolicFactorGraph fg(factorGraph);

	// eliminate it
  SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();

  CHECK(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST( SymbolicBayesNet, FromGaussian) {
  SymbolicBayesNet expected;
  expected.push_back(IndexConditional::shared_ptr(new IndexConditional(0, 1)));
  expected.push_back(IndexConditional::shared_ptr(new IndexConditional(1)));

  GaussianBayesNet gbn = createSmallGaussianBayesNet();
  SymbolicBayesNet actual(gbn);

  EXPECT(assert_equal(expected, actual));
}

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


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#include <iostream>
#include <boost/bind.hpp>
#include <fiber/fiber_control.hpp>
using namespace turi;

struct fibonacci_compute_promise {
  mutex* lock;
  size_t argument;
  size_t result;
  size_t parent_tid;
  bool result_set;
};

void fibonacci(fibonacci_compute_promise* promise) {
  //std::cout << promise->argument << "\n";
  if (promise->argument == 1 ||  promise->argument == 2) {
    promise->result = 1;
  } else {
    // recursive case
    mutex lock;
    fibonacci_compute_promise left, right;
    left.lock = &lock;
    left.argument = promise->argument - 1;
    left.result_set = false;
    left.parent_tid = fiber_control::get_tid();

    right.lock = &lock;
    right.argument = promise->argument - 2;
    right.result_set = false;
    right.parent_tid = fiber_control::get_tid();

    fiber_control::get_instance().launch(boost::bind(fibonacci, &left));
    fiber_control::get_instance().launch(boost::bind(fibonacci, &right));

    // wait on the left and right promise
    lock.lock();
    while (left.result_set == false || right.result_set == false) {
      fiber_control::deschedule_self(&lock.m_mut);
      lock.lock();
    }
    lock.unlock();

    assert(left.result_set);
    assert(right.result_set);
    promise->result = left.result + right.result;
  }
  promise->lock->lock();
  promise->result_set = true;
  if (promise->parent_tid) fiber_control::schedule_tid(promise->parent_tid);
  promise->lock->unlock();
}

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

  timer ti; ti.start();

  fibonacci_compute_promise promise;
  mutex lock;
  promise.lock = &lock;
  promise.result_set = false;
  promise.argument = 24;
  promise.parent_tid = 0;
  fiber_control::get_instance().launch(boost::bind(fibonacci, &promise));
  fiber_control::get_instance().join();
  assert(promise.result_set);
  std::cout << "Fib(" << promise.argument << ") = " << promise.result << "\n";

  std::cout << "Completion in " << ti.current_time() << "s\n";
  std::cout << fiber_control::get_instance().total_threads_created() << " threads created\n";
}