// Copyright (C) 2011  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <dlib/optimization.h>
#include <dlib/rand.h>

#include "tester.h"

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.find_max_factor_graph_nmplp");

// ----------------------------------------------------------------------------------------

    class map_problem
    {
        /*
            This is a simple 8 node problem with two cycles in it.
        */

    public:

        struct node_iterator
        {
            node_iterator() {}
            node_iterator(unsigned long nid_): nid(nid_) {}
            bool operator== (const node_iterator& item) const { return item.nid == nid; }
            bool operator!= (const node_iterator& item) const { return item.nid != nid; }

            node_iterator& operator++()
            {
                ++nid;
                return *this;
            }

            unsigned long nid;
        };

        struct neighbor_iterator
        {
            neighbor_iterator() : count(0) {}

            bool operator== (const neighbor_iterator& item) const { return item.node_id() == node_id(); }
            bool operator!= (const neighbor_iterator& item) const { return item.node_id() != node_id(); }
            neighbor_iterator& operator++()
            {
                ++count;
                return *this;
            }

            unsigned long node_id () const
            {
                if (home_node < 4)
                {
                    if (count == 0)
                        return (home_node + 4 + 1)%4;
                    else if (count == 1)
                        return (home_node + 4 - 1)%4;
                    else
                        return 8; // one past the end
                }
                else
                {
                    if (count == 0)
                        return (home_node + 4 + 1)%4 + 4;
                    else if (count == 1)
                        return (home_node + 4 - 1)%4 + 4;
                    else
                        return 8; // one past the end
                }
            }

            unsigned long home_node;
            unsigned long count;
        };

        unsigned long number_of_nodes (
        ) const
        {
            return 8;
        }

        node_iterator begin(
        ) const
        {
            node_iterator temp;
            temp.nid = 0;
            return temp;
        }

        node_iterator end(
        ) const
        {
            node_iterator temp;
            temp.nid = 8;
            return temp;
        }

        neighbor_iterator begin(
            const node_iterator& it
        ) const
        {
            neighbor_iterator temp;
            temp.home_node = it.nid;
            return temp;
        }

        neighbor_iterator begin(
            const neighbor_iterator& it
        ) const
        {
            neighbor_iterator temp;
            temp.home_node = it.node_id();
            return temp;
        }

        neighbor_iterator end(
            const node_iterator&
        ) const
        {
            neighbor_iterator temp;
            temp.home_node = 8;
            temp.count = 8;
            return temp;
        }

        neighbor_iterator end(
            const neighbor_iterator&
        ) const
        {
            neighbor_iterator temp;
            temp.home_node = 8;
            temp.count = 8;
            return temp;
        }

        unsigned long node_id (
            const node_iterator& it
        ) const
        {
            return it.nid;
        }

        unsigned long node_id (
            const neighbor_iterator& it
        ) const
        {
            return it.node_id();
        }

        unsigned long num_states (
            const node_iterator&
        ) const
        {
            return 2;
        }

        unsigned long num_states (
            const neighbor_iterator&
        ) const
        {
            return 2;
        }

        double factor_value (const node_iterator& it1, const node_iterator& it2, unsigned long s1, unsigned long s2) const
        { return basic_factor_value(it1.nid, it2.nid, s1, s2); }
        double factor_value (const neighbor_iterator& it1, const node_iterator& it2, unsigned long s1, unsigned long s2) const
        { return basic_factor_value(it1.node_id(), it2.nid, s1, s2); }
        double factor_value (const node_iterator& it1, const neighbor_iterator& it2, unsigned long s1, unsigned long s2) const
        { return basic_factor_value(it1.nid, it2.node_id(), s1, s2); }
        double factor_value (const neighbor_iterator& it1, const neighbor_iterator& it2, unsigned long s1, unsigned long s2) const
        { return basic_factor_value(it1.node_id(), it2.node_id(), s1, s2); }

    private:

        double basic_factor_value (
            unsigned long n1,
            unsigned long n2,
            unsigned long s1,
            unsigned long s2
        ) const
        {
            if (n1 > n2)
            {
                swap(n1,n2);
                swap(s1,s2);
            }

            if (n1 == 0 && n2 == 1)
            {
                if (s1 && s2)   return 100;
                if (!s1 && s2)  return 0;
                if (s1 && !s2)  return 0;
                if (!s1 && !s2) return 0;
            }
            if (n1 == 1 && n2 == 2)
            {
                if (s1 && s2)   return 5;
                if (!s1 && s2)  return 9;
                if (s1 && !s2)  return -100;
                if (!s1 && !s2) return 9;
            }
            if (n1 == 2 && n2 == 3)
            {
                if (s1 && s2)   return 10;
                if (!s1 && s2)  return 0;
                if (s1 && !s2)  return 10;
                if (!s1 && !s2) return 10;
            }
            if (n1 == 0 && n2 == 3)
            {
                if (s1 && s2)   return -5;
                if (!s1 && s2)  return 0;
                if (s1 && !s2)  return 0;
                if (!s1 && !s2) return 0;
            }

            if (n1 == 4 && n2 == 5)
            {
                if (s1 && s2)   return -100;
                if (!s1 && s2)  return 0;
                if (s1 && !s2)  return 0;
                if (!s1 && !s2) return 0;
            }
            if (n1 == 5 && n2 == 6)
            {
                if (s1 && s2)   return -5;
                if (!s1 && s2)  return -9;
                if (s1 && !s2)  return 100;
                if (!s1 && !s2) return -9;
            }
            if (n1 == 6 && n2 == 7)
            {
                if (s1 && s2)   return -10;
                if (!s1 && s2)  return 0;
                if (s1 && !s2)  return -10;
                if (!s1 && !s2) return -10;
            }
            if (n1 == 4 && n2 == 7)
            {
                if (s1 && s2)   return 5;
                if (!s1 && s2)  return 0;
                if (s1 && !s2)  return 0;
                if (!s1 && !s2) return 0;
            }

            DLIB_CASSERT(false, "n1: "<< n1 << "  n2: "<< n2 << "  s1: "<< s1 << "  s2: "<< s2);
            return 0;
        }

    };

// ----------------------------------------------------------------------------------------

    dlib::rand rnd;

    class map_problem2
    {
        /*
            This is a simple tree structured graph.  In particular, it is a star made
            up of 6 nodes.
        */
    public:
        matrix<double> numbers;

        map_problem2()
        {
            numbers = randm(5,3,rnd);
        }

        struct node_iterator
        {
            node_iterator() {}
            node_iterator(unsigned long nid_): nid(nid_) {}
            bool operator== (const node_iterator& item) const { return item.nid == nid; }
            bool operator!= (const node_iterator& item) const { return item.nid != nid; }

            node_iterator& operator++()
            {
                ++nid;
                return *this;
            }

            unsigned long nid;
        };

        struct neighbor_iterator
        {
            neighbor_iterator() : count(0) {}

            bool operator== (const neighbor_iterator& item) const { return item.node_id() == node_id(); }
            bool operator!= (const neighbor_iterator& item) const { return item.node_id() != node_id(); }
            neighbor_iterator& operator++()
            {
                ++count;
                return *this;
            }

            unsigned long node_id () const
            {
                if (home_node == 6)
                    return 6;

                if (home_node < 5)
                {
                    // all the nodes are connected to node 5 and nothing else
                    if (count == 0)
                        return 5;
                    else
                        return 6; // the number returned by the end() functions.
                }
                else if (count < 5)
                {
                    return count;
                }
                else
                {
                    return 6;
                }

            }

            unsigned long home_node;
            unsigned long count;
        };

        unsigned long number_of_nodes (
        ) const
        {
            return 6;
        }

        node_iterator begin(
        ) const
        {
            node_iterator temp;
            temp.nid = 0;
            return temp;
        }

        node_iterator end(
        ) const
        {
            node_iterator temp;
            temp.nid = 6;
            return temp;
        }

        neighbor_iterator begin(
            const node_iterator& it
        ) const
        {
            neighbor_iterator temp;
            temp.home_node = it.nid;
            return temp;
        }

        neighbor_iterator begin(
            const neighbor_iterator& it
        ) const
        {
            neighbor_iterator temp;
            temp.home_node = it.node_id();
            return temp;
        }

        neighbor_iterator end(
            const node_iterator&
        ) const
        {
            neighbor_iterator temp;
            temp.home_node = 6;
            return temp;
        }

        neighbor_iterator end(
            const neighbor_iterator&
        ) const
        {
            neighbor_iterator temp;
            temp.home_node = 6;
            return temp;
        }

        unsigned long node_id (
            const node_iterator& it
        ) const
        {
            return it.nid;
        }

        unsigned long node_id (
            const neighbor_iterator& it
        ) const
        {
            return it.node_id();
        }

        unsigned long num_states (
            const node_iterator&
        ) const
        {
            return 3;
        }

        unsigned long num_states (
            const neighbor_iterator&
        ) const
        {
            return 3;
        }

        double factor_value (const node_iterator& it1, const node_iterator& it2, unsigned long s1, unsigned long s2) const
        { return basic_factor_value(it1.nid, it2.nid, s1, s2); }
        double factor_value (const neighbor_iterator& it1, const node_iterator& it2, unsigned long s1, unsigned long s2) const
        { return basic_factor_value(it1.node_id(), it2.nid, s1, s2); }
        double factor_value (const node_iterator& it1, const neighbor_iterator& it2, unsigned long s1, unsigned long s2) const
        { return basic_factor_value(it1.nid, it2.node_id(), s1, s2); }
        double factor_value (const neighbor_iterator& it1, const neighbor_iterator& it2, unsigned long s1, unsigned long s2) const
        { return basic_factor_value(it1.node_id(), it2.node_id(), s1, s2); }

    private:

        double basic_factor_value (
            unsigned long n1,
            unsigned long n2,
            unsigned long s1,
            unsigned long s2
        ) const
        {
            if (n1 > n2)
            {
                swap(n1,n2);
                swap(s1,s2);
            }

            // basically ignore the other node in this factor.  The node we
            // are ignoring is the center node of this star graph.  So we basically
            // let it always have a value of 0.
            if (s2 == 0)
                return numbers(n1,s1) + 1;
            else
                return numbers(n1,s1);
        }

    };

// ----------------------------------------------------------------------------------------

    template <typename map_problem>
    double find_total_score (
        const map_problem& prob,
        const std::vector<unsigned long>& map_assignment
    )
    {
        typedef typename map_problem::node_iterator node_iterator;
        typedef typename map_problem::neighbor_iterator neighbor_iterator;

        double score = 0;
        for (node_iterator i = prob.begin(); i != prob.end(); ++i)
        {
            const unsigned long id_i = prob.node_id(i);
            for (neighbor_iterator j = prob.begin(i); j != prob.end(i); ++j)
            {
                const unsigned long id_j = prob.node_id(j);
                score += prob.factor_value(i,j, map_assignment[id_i], map_assignment[id_j]);
            }
        }

        return score;
    }

// ----------------------------------------------------------------------------------------

    template <
        typename map_problem
        >
    void brute_force_find_max_factor_graph_nmplp (
        const map_problem& prob,
        std::vector<unsigned long>& map_assignment
    )
    {
        std::vector<unsigned long> temp_assignment;
        temp_assignment.resize(prob.number_of_nodes(),0);

        double best_score = -std::numeric_limits<double>::infinity();

        for (unsigned long i = 0; i < 255; ++i)
        {
            temp_assignment[0] = (i&0x01)!=0;
            temp_assignment[1] = (i&0x02)!=0;
            temp_assignment[2] = (i&0x04)!=0;
            temp_assignment[3] = (i&0x08)!=0;
            temp_assignment[4] = (i&0x10)!=0;
            temp_assignment[5] = (i&0x20)!=0;
            temp_assignment[6] = (i&0x40)!=0;
            temp_assignment[7] = (i&0x80)!=0;

            double score = find_total_score(prob,temp_assignment);
            if (score > best_score)
            {
                best_score = score;
                map_assignment = temp_assignment;
            }
        }
    }

// ----------------------------------------------------------------------------------------

    template <typename map_problem>
    void do_test(
    )
    {
        print_spinner();
        std::vector<unsigned long> map_assignment1, map_assignment2;
        map_problem prob;
        find_max_factor_graph_nmplp(prob, map_assignment1, 1000, 1e-8);

        const double score1 = find_total_score(prob, map_assignment1);

        brute_force_find_max_factor_graph_nmplp(prob, map_assignment2);
        const double score2 = find_total_score(prob, map_assignment2);

        dlog << LINFO << "score NMPLP: " << score1;
        dlog << LINFO << "score MAP:   " << score2;

        DLIB_TEST(std::abs(score1 - score2) < 1e-10);
        DLIB_TEST(mat(map_assignment1) == mat(map_assignment2));
    }

// ----------------------------------------------------------------------------------------

    template <typename map_problem>
    void do_test2(
    )
    {
        print_spinner();
        std::vector<unsigned long> map_assignment1, map_assignment2;
        map_problem prob;
        find_max_factor_graph_nmplp(prob, map_assignment1, 10, 1e-8);

        const double score1 = find_total_score(prob, map_assignment1);

        map_assignment2.resize(6);
        map_assignment2[0] = index_of_max(rowm(prob.numbers,0));
        map_assignment2[1] = index_of_max(rowm(prob.numbers,1));
        map_assignment2[2] = index_of_max(rowm(prob.numbers,2));
        map_assignment2[3] = index_of_max(rowm(prob.numbers,3));
        map_assignment2[4] = index_of_max(rowm(prob.numbers,4));
        map_assignment2[5] = 0;
        const double score2 = find_total_score(prob, map_assignment2);

        dlog << LINFO << "score NMPLP: " << score1;
        dlog << LINFO << "score MAP:   " << score2;
        dlog << LINFO << "MAP assignment: "<< trans(mat(map_assignment1));

        DLIB_TEST(std::abs(score1 - score2) < 1e-10);
        DLIB_TEST(mat(map_assignment1) == mat(map_assignment2));
    }

// ----------------------------------------------------------------------------------------

    class test_find_max_factor_graph_nmplp : public tester
    {
    public:
        test_find_max_factor_graph_nmplp (
        ) :
            tester ("test_find_max_factor_graph_nmplp",
                    "Runs tests on the find_max_factor_graph_nmplp routine.")
        {}

        void perform_test (
        )
        {
            do_test<map_problem>();

            for (int i = 0; i < 10; ++i)
                do_test2<map_problem2>();
        }
    } a;

}


#ifndef __WALD_LM_METHOD_H__
#define __WALD_LM_METHOD_H__

#include <string>
#include <vector>

#include <armadillo>

#include <besiq/method/method.hpp>
#include <besiq/stats/log_scale.hpp>

/**
 * This class is responsible for executing the closed form
 * wald test for a logistic regression model.
 */
class wald_lm_method
: public method_type
{
public:
    /**
     * Constructor.
     *
     * @param data Additional data required by all methods, such as
     *             covariates.
     * @param unequal_var If true, estimates a separate variance for each cell.
     */
    wald_lm_method(method_data_ptr data, bool unequal_var = false);

    /**
     * @see method_type::init.
     */
    virtual std::vector<std::string> init();

    /**
     * Returns the last computed covariance matrix.
     *
     * @return the last computed covariance matrix.
     */
    arma::mat get_last_C();

    /**
     * Returns the last computed beta.
     *
     * @return the last computed beta.
     */
    arma::vec get_last_beta();

    /**
     * @see method_type::run.
     */
    virtual double run(const snp_row &row1, const snp_row &row2, float *output);
private:
    /**
     * Weight for each sample.
     */
    arma::vec m_weight;

    /**
     * Phenotype.
     */
    arma::vec m_pheno;

    /**
     * Missing samples.
     */
    arma::uvec m_missing;

    /**
     * Determines whether variances should be estimated separately.
     */
    bool m_unequal_var;

    /**
     * Current covariance matrix for the betas.
     */
    arma::mat m_C;

    /**
     * Current betas.
     */
    arma::vec m_beta;
};

#endif /* End of __WALD_LM_METHOD_H__ */


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_tool.hh"

#include <boost/python.hpp>
#include "numpy_bind.hh"
#include "hash_map_wrap.hh"

#include <boost/math/special_functions/gamma.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

void collect_vertex_marginals(GraphInterface& gi, boost::any ob,
                              boost::any op, size_t update)
{
    typedef vprop_map_t<int32_t>::type vmap_t;
    auto b = any_cast<vmap_t>(ob).get_unchecked();

    run_action<>()
        (gi, [&](auto& g, auto p)
         {
             typename property_traits<decltype(p)>::value_type::value_type
                 up = update;
             parallel_vertex_loop
                 (g,
                  [&](auto v)
                  {
                      auto r = b[v];
                      auto& pv = p[v];
                      if (pv.size() <= size_t(r))
                          pv.resize(r + 1);
                      pv[r] += up;
                  });
         },
         vertex_scalar_vector_properties())(op);
}

class BlockPairHist
    : public gt_hash_map<std::pair<int32_t,int32_t>,size_t>
{
public:

    boost::python::dict get_state()
    {
        boost::python::dict state;
        for (auto& kv : *this)
        {
            auto k = python::make_tuple(kv.first.first,
                                        kv.first.second);
            state[k] = kv.second;
        }
        return state;
    }

    void set_state(boost::python::dict state)
    {
        auto keys = state.keys();
        for (int i = 0; i < python::len(keys); ++i)
        {
            auto k = keys[i];
            int32_t r = python::extract<int32_t>(k[0]);
            int32_t s = python::extract<int32_t>(k[1]);
            size_t v = python::extract<size_t>(state[k]);
            (*this)[make_pair(r, s)] = v;
        }
    }

    size_t get_item(boost::python::object k)
    {
        int32_t r = python::extract<int32_t>(k[0]);
        int32_t s = python::extract<int32_t>(k[1]);
        auto iter = this->find(make_pair(r, s));
        if (iter == this->end())
            return 0;
        return iter->second;
    }

    void set_item(boost::python::object k, double v)
    {
        int32_t r = python::extract<int32_t>(k[0]);
        int32_t s = python::extract<int32_t>(k[1]);
        (*this)[make_pair(r, s)] = v;
    }

};

void collect_edge_marginals(GraphInterface& gi, boost::any ob,
                            boost::any op, size_t update)
{
    typedef vprop_map_t<int32_t>::type vmap_t;
    auto b = any_cast<vmap_t>(ob).get_unchecked();

    typedef eprop_map_t<python::object>::type
        emap_t;
    auto pe = any_cast<emap_t>(op).get_unchecked(gi.get_edge_index_range());

    run_action<>()
        (gi,
         [&](auto& g)
         {
            parallel_edge_loop
                 (g,
                  [&](const auto& e)
                  {
                      auto u = min(source(e, g), target(e, g));
                      auto v = max(source(e, g), target(e, g));

                      auto r = b[u];
                      auto s = b[v];

                      BlockPairHist& h =
                          boost::python::extract<BlockPairHist&>(pe[e]);

                      h[make_pair(r, s)] += update;
                  });
         })();
}

double mf_entropy(GraphInterface& gi, boost::any opv)
{
    double H=0;
    run_action<>()
        (gi,
         [&](auto& g, auto pv)
         {
             for (auto v : vertices_range(g))
             {
                 double sum = 0;
                 for (auto p : pv[v])
                     sum += p;
                 for (double p : pv[v])
                 {
                     if (p == 0)
                         continue;
                     p /= sum;
                     H -= p * log(p);
                 }
             }
         },
         vertex_scalar_vector_properties())(opv);

    return H;
}

boost::python::tuple bethe_entropy(GraphInterface& gi, boost::any op,
                                   boost::any opv)
{
    typedef vprop_map_t<vector<double>>::type vmap_t;
    vmap_t pv = any_cast<vmap_t>(opv);

    typedef eprop_map_t<boost::python::object>::type emap_t;
    auto pe = any_cast<emap_t>(op).get_unchecked();

    double H=0, Hmf=0;
    run_action<>()
        (gi,
         [&](auto& g)
         {
             for (auto e : edges_range(g))
             {
                 auto u = min(source(e, g), target(e, g));
                 auto v = max(source(e, g), target(e, g));

                 double sum = 0;

                 BlockPairHist& h =
                     boost::python::extract<BlockPairHist&>(pe[e]);

                 for (auto& prs : h)
                 {
                     sum += prs.second;
                     size_t r = prs.first.first;
                     size_t s = prs.first.second;
                     if (r >= pv[u].size())
                         pv[u].resize(r + 1);
                     pv[u][r] += prs.second;
                     if (s >= pv[v].size())
                         pv[v].resize(s + 1);
                     pv[v][s] += prs.second;
                 }

                 for (auto& prs : h)
                 {
                     if (prs.second == 0)
                         continue;
                     double pi = prs.second / sum;
                     H -= pi * log(pi);
                 }
             }

             for (auto v : vertices_range(g))
             {
                 double sum = std::accumulate(pv[v].begin(), pv[v].end(), 0.);
                 for (double pi : pv[v])
                 {
                     if (pi == 0)
                         continue;
                     pi /= sum;
                     int kt = 1 - total_degreeS()(v, g);
                     if (kt != 0)
                         H -= kt * (pi * log(pi));
                     Hmf -= pi * log(pi);
                 }
             }
         })();

    return boost::python::make_tuple(H, Hmf);
}

class PartitionHist
    : public gt_hash_map<std::vector<int32_t>, double>
{
public:

    boost::python::dict get_state()
    {
        boost::python::dict state;
        for (auto& kv : *this)
            state[kv.first] = kv.second;
        return state;
    }

    void set_state(boost::python::dict state)
    {
        auto keys = state.keys();
        for (int i = 0; i < python::len(keys); ++i)
        {
            auto& k = python::extract<std::vector<int32_t>&>(keys[i])();
            double v = python::extract<double>(state[k]);
            (*this)[k] = v;
        }
    }

    size_t get_item(std::vector<int32_t>& k)
    {
        auto iter = this->find(k);
        if (iter == this->end())
            return 0;
        return iter->second;
    }

    void set_item(std::vector<int32_t>& k, double v)
    {
        (*this)[k] = v;
    }

};

double log_n_permutations(const vector<int32_t>& b)
{
    std::vector<int32_t> count(b.size());
    for (auto bi : b)
        count[bi]++;
    double n = boost::math::lgamma(b.size() + 1);
    for (auto nr : count)
        n -= boost::math::lgamma(nr + 1);
    return n;
}

vector<int32_t> unlabel_partition(vector<int32_t> b)
{
    std::vector<int32_t> map(b.size(), -1);
    size_t pos = 0;
    for (auto& bi : b)
    {
        auto& x = map[bi];
        if (x == -1)
        {
            x = pos;
            ++pos;
        }
        bi = x;
    }
    return b;
}

void collect_partitions(boost::any& ob, PartitionHist& h, double update,
                        bool unlabel)
{
    typedef vprop_map_t<int32_t>::type vmap_t;
    auto& b = any_cast<vmap_t&>(ob);
    auto& v = b.get_storage();
    if (unlabel)
    {
        auto vc = unlabel_partition(v);
        h[vc] += update;
    }
    else
    {
        h[v] += update;
    }
}

void collect_hierarchical_partitions(python::object ovb, PartitionHist& h,
                                     size_t update, bool unlabel)
{
    typedef vprop_map_t<int32_t>::type vmap_t;
    vector<int32_t> v;
    for (int i = 0; i < len(ovb); ++i)
    {
        boost::any& ob = python::extract<boost::any&>(ovb[i])();
        auto& b = any_cast<vmap_t&>(ob);
        auto& vi = b.get_storage();
        v.reserve(v.size() + vi.size());
        if (unlabel)
        {
            auto vc = unlabel_partition(v);
            v.insert(v.end(), vc.begin(), vc.end());
        }
        else
        {
            v.insert(v.end(), vi.begin(), vi.end());
        }
        v.push_back(-1);
    }
    h[v] += update;
}

double partitions_entropy(PartitionHist& h, bool unlabeled)
{
    double S = 0;
    size_t N = 0;
    for (auto kv : h)
    {
        if (kv.second == 0)
            continue;
        N += kv.second;
        S -= kv.second * log(kv.second);
        if (unlabeled)
            S += kv.second * log_n_permutations(kv.first);
    }
    if (N > 0)
    {
        S /= N;
        S += log(N);
    }
    return S;
}

void export_marginals()
{
    using namespace boost::python;

    class_<BlockPairHist>("BlockPairHist",
                          "Histogram of block pairs, implemented in C++.\n"
                          "Interface supports querying and setting using pairs "
                          "of ints as keys, and ints as values.")
        .def("__setitem__", &BlockPairHist::set_item)
        .def("__getitem__", &BlockPairHist::get_item)
        .def("__setstate__", &BlockPairHist::set_state)
        .def("__getstate__", &BlockPairHist::get_state)
        .def("asdict", &BlockPairHist::get_state,
             "Return the histogram's contents as a dict.").enable_pickling();

    class_<PartitionHist>("PartitionHist",
                          "Histogram of partitions, implemented in C++.\n"
                          "Interface supports querying and setting using Vector_int32_t "
                          "as keys, and ints as values.")
        .def("__setitem__", &PartitionHist::set_item)
        .def("__getitem__", &PartitionHist::get_item)
        .def("__setstate__", &PartitionHist::set_state)
        .def("__getstate__", &PartitionHist::get_state)
        .def("asdict", &PartitionHist::get_state,
             "Return the histogram's contents as a dict.").enable_pickling();

    def("vertex_marginals", &collect_vertex_marginals);
    def("edge_marginals", &collect_edge_marginals);
    def("mf_entropy", &mf_entropy);
    def("bethe_entropy", &bethe_entropy);
    def("collect_partitions", &collect_partitions);
    def("collect_hierarchical_partitions", &collect_hierarchical_partitions);
    def("partitions_entropy", &partitions_entropy);
}


#include <stdio.h>
#include <inttypes.h>
#include <fstream>
#include <iostream>
#include <vector>
#include <signal.h>
#include <getopt.h>

#include <lcm/lcm.h>
#include <bot_core/bot_core.h>
#include <bot_lcmgl_client/lcmgl.h>
#include <GL/gl.h>

#include <Eigen/Dense>

#include <lcmtypes/bot_core_pose_t.h>
#include <lcmtypes/bot_core_image_t.h>

namespace rv{

void draw_line_lcmgl(bot_lcmgl_t* lcmgl_, Eigen::Vector3d st_pt, Eigen::Vector3d end_pt,
        float r, float g, float b){

    // position

    bot_lcmgl_line_width(lcmgl_, 4.0f);
    bot_lcmgl_point_size(lcmgl_, 7.0f);
    bot_lcmgl_color3f(lcmgl_, r, g, b);

    bot_lcmgl_begin(lcmgl_, GL_LINE_STRIP);
    bot_lcmgl_vertex3d(lcmgl_, st_pt[0], st_pt[1], st_pt[2]);
    bot_lcmgl_vertex3d(lcmgl_,end_pt[0], end_pt[1], end_pt[2]);
    bot_lcmgl_end(lcmgl_);

}

void draw_maze(bot_lcmgl_t* lcmgl, std::vector<std::vector<float> > maze_segments){

    for(int i = 0; i < maze_segments.size(); i++){

        Eigen::Vector3d st_pt, end_pt;
        st_pt << maze_segments[i][0], maze_segments[i][1], 0;
        end_pt << maze_segments[i][2], maze_segments[i][3], 0;

        draw_line_lcmgl(lcmgl, st_pt, end_pt,
                0.5, 0.5, 0);

    }
}

void get_maze_segments(std::string csv_file, std::vector<std::vector<float> > & maze_segments){

	std::vector<float> segments, segments_t;
	std::ifstream file (csv_file);
	std::string value;

	std::vector<Eigen::Vector3d> holodeck_corners;
	Eigen::Vector3d ll, lr, tr, tl;
	ll << -2.575, -1.945, 0;
	lr << 2.565, -2,022, 0;
	tr << 2.480, 3.180, 0;
	tl << -2.588, 3.121, 0;

	double dx, dy, sx, sy;
	dx = 0.5 * (lr[0] - ll[0]) + 0.5 * (tr[0] - tl[0]);
	dy = 0.5 * (tr[1] - lr[1]) + 0.5 * (tl[1] - ll[1]);

	sx = dx / 4;
	sy = dy / 4;

	while ( file.good() )
	{

		float y1, x1, y2, x2;
		getline ( file, value, ',' );
		x1 = atof(value.c_str()) * sx + ll[1];
		getline ( file, value, ',' );
		y1 = (4 - atof(value.c_str())) * sy + ll[0];
		getline ( file, value, ',' );
		x2 = atof(value.c_str()) * sx + ll[1];
		getline ( file, value);
		y2 = (4 - atof(value.c_str())) * sy + ll[0];

		segments.push_back(y1);
		segments.push_back(x1);
		segments.push_back(y2);
		segments.push_back(x2);

		maze_segments.push_back(segments);

		segments.clear();
	}

}

} // end of namespace rv

static void
usage(const char *name)
{
    fprintf(
            stderr,
            "usage: %s [options]\n"
            "\n"
            "  -h, --help                            Shows this help text and exits\n"
            "  -m, --maze-id                         maze id (1-4)\n"
            "\n",
            name);

    exit(1);
}

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

    int maze_id = 1;

    const char *optstring = "hm:";
    int c;
    struct option long_opts[] =
    {
        { "help", no_argument, 0, 'h' },
        { "maze", required_argument, 0, 'm' },
        { 0, 0, 0, 0 }
    };

    while ((c = getopt_long(argc, argv, optstring, long_opts, 0)) >= 0) {
        switch (c){
            case 'm':
                maze_id = atoi(optarg);
                break;
            case 'h':
                usage(argv[0]);
                break;
        }
    }

    lcm_t* lcm_ = lcm_create(NULL);
    bot_lcmgl_t* lcmgl_maze = bot_lcmgl_init(lcm_, "LCMGL_MAZE");

    std::vector<std::vector<float> > maze_segments;

    std::stringstream ss_maze;
    ss_maze << "./config/maze/maze-" << maze_id << ".csv";
    rv::get_maze_segments(ss_maze.str(), maze_segments);
    std::cout << ss_maze.str() << " contains: "
        << maze_segments.size() << " segments." << std::endl;
    if(maze_segments.size() == 0){
        std:: cout << "check file path " << std::endl;
    }

    rv::draw_maze(lcmgl_maze, maze_segments);
    bot_lcmgl_switch_buffer(lcmgl_maze);

    return 0;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_POSMIN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_POSMIN_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-reduction
    Function object implementing posmin capabilities

    Returns the index of the first occurence of least element of the SIMD vector

    @par Semantic:

    For every parameter of type T

    @code
    std::size_t r = posmin(x);
    @endcode

    is similar to:

    @code
      T m =  minimum(x);
      std::size_t r;
      for(std::size_t i=0; i < cardinal_of<T>; i++)
        if (m == x[i]) { r = i;  break; }
    @endcode

  **/
  const boost::dispatch::functor<tag::posmin_> posmin = {};
} }
#endif

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

#endif


/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    This program is free software: you can redistribute it and/or modify
|    it under the terms of the GNU General Public License as published by
|    the Free Software Foundation, either version 3 of the License, or
|    (at your option) any later version.
|
|    This program is distributed in the hope that it will be useful,
|    but WITHOUT ANY WARRANTY; without even the implied warranty of
|    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
|    GNU General Public License for more details.
|
|    You should have received a copy of the GNU General Public License
|    along with this program.  If not, see <http://www.gnu.org/licenses/>.
|
\**************************************************************************/

#include <cmath>
#include <algorithm>
#include <map>
#include <set>

#include <hashclash/saveload_bz2.hpp>
#include <hashclash/sha1differentialpath.hpp>
#include <hashclash/sha1detail.hpp>
#include <hashclash/sha1messagespace.hpp>
#include <hashclash/booleanfunction.hpp>
#include <hashclash/progress_display.hpp>
#include <hashclash/rng.hpp>

#include <boost/lexical_cast.hpp>

#include "main.hpp"

	inline uint32 sha1_step_round1_bw(int t, uint32 Q[])
	{
		const int offset = 4;
		uint32 Ft = sha1_f1(Q[offset+t-1], rotate_left(Q[offset+t-2],30), rotate_left(Q[offset+t-3],30));
		return Q[offset+t+1] - ( Ft + sha1_ac[0] + rotate_left(Q[offset+t],5) + rotate_left(Q[offset+t-4],30) );
	}

struct filter_t
{
	const int Qoffset = 4;

	uint32 m[80];
	uint32 Q[85];

	uint32 Qcondmask[85];
	uint32 Qset1mask[85];
	uint32 Qprevmask[85];

	uint32 mcondmask[80];
	uint32 mset1mask[80];

	sha1differentialpath& path;
	parameters_type& parameters;
	filter_t(sha1differentialpath& _path, parameters_type& _parameters)
		: path(_path), parameters(_parameters)
	{
		for (int i = 0; i < 80; ++i) m[i] = 0;
		for (int i = 0; i < 80; ++i) mcondmask[i] = 0;
		for (int i = 0; i < 80; ++i) mset1mask[i] = 0;
		for (int i = 0; i < 85; ++i) Q[i] = 0;
		for (int i = 0; i < 85; ++i) Qcondmask[i] = 0;
		for (int i = 0; i < 85; ++i) Qset1mask[i] = 0;
		for (int i = 0; i < 85; ++i) Qprevmask[i] = 0;

//		show_path(path);
		for (int t = -4; t <= 20 && t < path.tend(); ++t)
		{
//			cout << "Q" << t << "\t";
			for (unsigned b = 0; b < 32; ++b)
			{
				bitcondition bc = path(t,b);

				if (bc != bc_constant)
				switch (bc)
				{
					case bc_one: case bc_minus:
						Qset1mask[Qoffset+t] |= 1<<b;
					case bc_zero: case bc_plus:
						Qcondmask[Qoffset+t] |= 1<<b;
						break;
					case bc_prevn:
						Qset1mask[Qoffset+t] |= 1<<b;
					case bc_prev:
						Qprevmask[Qoffset+t] |= 1<<b;
						Qcondmask[Qoffset+t] |= 1<<b;
						break;
					default:
						cout << "========= " << bc << " =======" << endl;
						throw;
				}
//				cout << bc;

			}

			if (t >= 0 && t < 20)
			{
				sdr mt = path.getme(t);
				mcondmask[t] = mt.mask;
				mset1mask[t] = mt.set1conditions();

//				cout << mt;
			}

//			cout << endl;
		}
	}

	typedef vector<uint32> sol_t;

	void gensols(vector<sol_t>& sols, int solt)
	{
		sols.clear();
		if (solt == -5)
		{
			sols.resize(1);
			return;
		}

		vector<sol_t> soltm1;
		gensols(soltm1, solt-1);

		sol_t sol;
		for (auto& s : soltm1)
		{
			sol = s;
			sol.push_back( 0 );

			uint32 set1 = Qset1mask[Qoffset+solt];
			if (solt > -4)
				set1 ^= (Qprevmask[Qoffset+solt] & sol[Qoffset+solt-1]);

			const uint32 mask = ~Qcondmask[Qoffset+solt];
			uint32 v = 0;
			do
			{
				--v;
				v &= mask;

				sol[Qoffset+solt] = v ^ set1;

				if (solt > 0)
				{
					uint32 m = sha1_step_round1_bw(solt-1, &sol[0]);
					if (((m^mset1mask[solt-1]) & mcondmask[solt-1]) != 0)
						continue;
				}

				sols.push_back(sol);

			} while (v != 0);

		}

//		cout << "# sols @ t=" << solt << ": " << sols.size() << endl;
	}

};

bool filterpath(sha1differentialpath& path, parameters_type& parameters)
{
	cleanup_path(path);

	filter_t filter(path, parameters);

	vector< filter_t::sol_t > sols;
	filter.gensols(sols, parameters.filtert);
	if (0 && !sols.empty())
	{
		show_path(path);
		cout << sols.size() << endl;
	}
	return ! sols.empty();
}

int filterfeasible(parameters_type& parameters)
{
	vector<sha1differentialpath> vecpath;
	cout << "Loading " << parameters.infile1 << "..." << flush;
	try {
		load_bz2(vecpath, binary_archive, parameters.infile1);
		cout << "done (loaded " << vecpath.size() << " paths)." << endl;
	} catch (...) {
		cout << "failed." << endl;
		return 1;
	}
	if (parameters.unique) {
		sort(vecpath.begin(), vecpath.end());
		vecpath.erase( unique(vecpath.begin(), vecpath.end()), vecpath.end() );
		cout << "Reduced to " << vecpath.size() << " unique paths." << endl;
	}

	uint64 cnt = 0, cnt2 = 0;
	for (std::size_t i = 0; i < vecpath.size();)
	{
		if (hw(++cnt) == 1)
			cout << "cnt = " << cnt << endl;
		if (filterpath(vecpath[i], parameters))
		{
			++i;
			if (hw(++cnt2) == 1)
			cout << "cnt2 = " << cnt2 << endl;

		}
		else
		{
			if (i != vecpath.size()-1)
				std::swap(vecpath[i], vecpath.back());
			vecpath.pop_back();
		}
	}
	cout << "Differential paths remaining: " << vecpath.size() << endl;
	if (!parameters.outfile1.empty())
	{
		cout << "Saving " << parameters.outfile1 << "..." << flush;
		save_bz2(vecpath, binary_archive, parameters.outfile1);
		cout << "done." << endl;
	}
	return 0;
}

int selectpath(parameters_type& parameters)
{
	vector<sha1differentialpath> vecpath;
	cout << "Loading " << parameters.infile1 << "..." << flush;
	try {
		load_bz2(vecpath, binary_archive, parameters.infile1);
		cout << "done (loaded " << vecpath.size() << " paths)." << endl;
	} catch (...) {
		cout << "failed." << endl;
		return 1;
	}
	if (parameters.seli < vecpath.size())
	{
		sha1differentialpath path = vecpath[parameters.seli];
		if (parameters.invert)
		{
			for (int t = path.tbegin(); t < path.tend(); ++t)
			{
				if (t-4 >= path.tbegin() && t+1 < path.tend())
					path.getme(t) = -path.getme(t);
				for (unsigned b = 0; b < 32; ++b)
				{
					bitcondition bc = path(t,b);
					if (bc == bc_plus)
						path.setbitcondition(t,b,bc_minus);
					else if (bc == bc_minus)
						path.setbitcondition(t,b,bc_plus);
				}
			}
		}
		show_path(path);
		cout << "Saving " << parameters.outfile1 << "..." << flush;
		save_bz2(path, binary_archive, parameters.outfile1);
		cout << "done." << endl;
		return 0;
	}
	cout << "Index is out of bounds." << endl;
	return 1;
}


//=========================================================================
//
// Copyright 2019 Kitware, Inc.
// Author: Guilbert Pierre (spguilbert@gmail.com)
// Date: 05-06-2019
//
// 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.
//=========================================================================

// VTK
#include <vtkXMLPolyDataReader.h>
#include <vtkXMLPolyDataWriter.h>
#include <vtkNew.h>
#include <vtkPolyData.h>
#include <vtkPointData.h>
#include <vtkDataArray.h>
#include <vtkDoubleArray.h>
#include <vtkSmartPointer.h>

// EIGEN
#include <Eigen/Dense>

// PCL
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

// LOCAL
#include "vtkEigenTools.h"
#include "TrajectoryReoptimization.h"
#include "CameraCalibration.h"
#include "CameraProjection.h"
#include "vtkEigenTools.h"
#include "vtkPCLConversions.h"
#include "vtkConversions.h"
#include "RegistrationTools.h"
#include "vtkTemporalTransforms.h"
#include "vtkCustomTransformInterpolator.h"
#include "CeresTools.h"
#include "vtkConversions.h"

//----------------------------------------------------------------------------
int TestRelocation(std::string referenceFilename, std::string toAlignFilename)
{
  // Load reference point cloud data
  vtkNew<vtkXMLPolyDataReader> readerCloudReference;
  readerCloudReference->SetFileName(referenceFilename.c_str());
  readerCloudReference->Update();
  vtkSmartPointer<vtkPolyData> vtkReferenceCloud = readerCloudReference->GetOutput();

  // load to align cloud data
  vtkNew<vtkXMLPolyDataReader> readerCloudToAligned;
  readerCloudToAligned->SetFileName(toAlignFilename.c_str());
  readerCloudToAligned->Update();
  vtkSmartPointer<vtkPolyData> vtkToAlignedCloud = readerCloudToAligned->GetOutput();

  // Convert vtk data into pcl
  pcl::PointCloud<pcl::PointXYZ>::Ptr pclReferenceCloud = vtkPCLConversions::PointCloudFromPolyData(vtkReferenceCloud);
  pcl::PointCloud<pcl::PointXYZ>::Ptr pclToAlignedCloud = vtkPCLConversions::PointCloudFromPolyData(vtkToAlignedCloud);

  // Apply a voxel grid filter since the density of point
  // is not homogene
  double leaf = 0.6;
  pcl::PointCloud<pcl::PointXYZ>::Ptr filteredReference(new pcl::PointCloud<pcl::PointXYZ>());
  pcl::PointCloud<pcl::PointXYZ>::Ptr filteredToAligned(new pcl::PointCloud<pcl::PointXYZ>());
  pcl::VoxelGrid<pcl::PointXYZ> voxelGridFilter;
  voxelGridFilter.setLeafSize(leaf, leaf, leaf);
  voxelGridFilter.setInputCloud(pclReferenceCloud);
  voxelGridFilter.filter(*filteredReference);
  voxelGridFilter.setInputCloud(pclToAlignedCloud);
  voxelGridFilter.filter(*filteredToAligned);

  // Compute the relocation
  Eigen::Matrix4d H = Eigen::Matrix4d::Identity();
  std::vector<bool> pointsUsed;
  Eigen::Matrix4d Href = ICPPointToPlaneRegistration(filteredReference, filteredToAligned, H, pointsUsed, 10, 30);

  // compare obtained relocation and expected relocation
  Eigen::Matrix<double, 6, 1> DoF6Params;;
  DoF6Params.block(0, 0, 3, 1) = MatrixToRollPitchYaw(Href.block(0, 0, 3, 3));
  DoF6Params.block(3, 0, 3, 1) = Href.block(0, 3, 3, 1);;
  Eigen::Matrix<double, 6, 1> ExpectedDoF6Params;
  ExpectedDoF6Params << 0.00673364, 0.00140092, -0.0648394, 1.78213, 5.36607, -0.796278;

  double epsilon = 1e-2;
  for (int i = 0; i < 6; ++i)
  {
    if (std::abs(DoF6Params[i] - ExpectedDoF6Params[i]) / std::abs(ExpectedDoF6Params[i]) > epsilon)
    {
      std::cout << "got: " << DoF6Params[i] << " expected: " << ExpectedDoF6Params[i] << std::endl;
      return 1;
    }
  }

  return 0;
}

//----------------------------------------------------------------------------
int TestTrajectoryReoptimizationLoopClosure(std::string trajFileName, std::string expectedReoptimizedTraj)
{
  int errors = 0;

  // read input data
  vtkNew<vtkXMLPolyDataReader> reader;
  reader->SetFileName(trajFileName.c_str());
  reader->Update();

  // get the trajectory
  vtkSmartPointer<vtkPolyData> inputTraj = reader->GetOutput();

  // Anchor computed from relocation after a loop-closure
  Eigen::Matrix4d Hanchor = Eigen::Matrix4d::Identity();
  Hanchor <<     0.997898,   0.0648019,  0.000961645,    1.78213,
               -0.0647939,    0.997875,  -0.00681021,    5.36607,
              -0.00140092,  0.00673359,     0.999976,  -0.796278,
                        0,           0,            0,          1;

  // populate the poses
  PoseEstimationVector absolutePoses;
  ConvertPolyDataToPoseEstimation(inputTraj, absolutePoses);

  Hanchor = Hanchor * absolutePoses[absolutePoses.size() - 1].H;

  // Optimization
  PoseEstimationVector correctedPoses;
  correctedPoses = TrajectoryReoptimization(absolutePoses, Hanchor, inputTraj->GetNumberOfPoints() - 1, 3);

  // create arrays
  vtkNew<vtkDoubleArray> mahalanobis;
  vtkNew<vtkDoubleArray> anglesDist;
  vtkNew<vtkDoubleArray> positionDist;
  // name
  mahalanobis->SetName("Mahalanobis distance");
  anglesDist->SetName("Angle distance");
  positionDist->SetName("Position distance");
  // components
  mahalanobis->SetNumberOfComponents(1);
  anglesDist->SetNumberOfComponents(1);
  positionDist->SetNumberOfComponents(1);
  // Allocate
  mahalanobis->SetNumberOfTuples(inputTraj->GetNumberOfPoints());
  anglesDist->SetNumberOfTuples(inputTraj->GetNumberOfPoints());
  positionDist->SetNumberOfTuples(inputTraj->GetNumberOfPoints());

  // Convert to vtk
  vtkNew<vtkPolyData> correctedTraj;
  correctedTraj->DeepCopy(inputTraj);
  vtkDataArray* orientation = correctedTraj->GetPointData()->GetArray("Orientation(AxisAngle)");
  for (int poseIndex = 0; poseIndex < inputTraj->GetNumberOfPoints(); ++poseIndex)
  {
    // Orientation
    Eigen::Matrix3d R = correctedPoses[poseIndex].H.block(0, 0, 3, 3);
    Eigen::AngleAxisd angleAxis(R);
    orientation->SetTuple4(poseIndex, angleAxis.axis()(0), angleAxis.axis()(1), angleAxis.axis()(2), angleAxis.angle());

    // Pose
    Eigen::Vector3d T = correctedPoses[poseIndex].H.block(0, 3, 3, 1);
    correctedTraj->GetPoints()->SetPoint(poseIndex, T.data());

    // Fill reoptimization information
    mahalanobis->SetTuple1(poseIndex, correctedPoses[poseIndex].MahalanobisDistance);
    anglesDist->SetTuple1(poseIndex, correctedPoses[poseIndex].AnglesDistance);
    positionDist->SetTuple1(poseIndex, correctedPoses[poseIndex].PositionDistance);
  }
  correctedTraj->GetPointData()->AddArray(mahalanobis);
  correctedTraj->GetPointData()->AddArray(anglesDist);
  correctedTraj->GetPointData()->AddArray(positionDist);

  // Load the expected result
  reader->SetFileName(expectedReoptimizedTraj.c_str());
  reader->Update();
  vtkSmartPointer<vtkPolyData> expectedTraj = reader->GetOutput();
  vtkDataArray* expectedOrientation = inputTraj->GetPointData()->GetArray("Orientation(AxisAngle)");

  // Compare the results
  double epsilon = 1e-6;
  for (int pointIdx = 0; pointIdx < expectedTraj->GetNumberOfPoints(); ++pointIdx)
  {
    double pt0[3], pt1[3];
    correctedTraj->GetPoint(pointIdx, pt0);
    expectedTraj->GetPoint(pointIdx, pt1);

    for (int i = 0; i < 3; ++i)
    {
      if (std::abs(pt0[i] - pt1[i]) / std::abs(pt1[i]) > epsilon)
      {
        std::cout << "error, expected: " << pt1[i] << " got: " << pt0[i] << std::endl;
        return 1;
      }
    }
  }

  return errors;
}

//----------------------------------------------------------------------------
int TestTrajectoryReoptimizationGPSIMU(std::string trajFileName, std::string gpsIMUTrajFileName)
{
  int errors = 0;

  // SLAM trajectory
  vtkNew<vtkXMLPolyDataReader> reader;
  reader->SetFileName(trajFileName.c_str());
  reader->Update();
  vtkSmartPointer<vtkPolyData> slamTraj = reader->GetOutput();

  // GPS / IMU trajectory
  vtkNew<vtkXMLPolyDataReader> reader2;
  reader2->SetFileName(gpsIMUTrajFileName.c_str());
  reader2->Update();
  vtkSmartPointer<vtkPolyData> imuTraj = reader2->GetOutput();

  // Create a linear interpolator from the imu trajectory
  vtkSmartPointer<vtkTemporalTransforms> imuTransforms = vtkTemporalTransforms::CreateFromPolyData(imuTraj);
  vtkSmartPointer<vtkCustomTransformInterpolator> imuInterp = imuTransforms->CreateInterpolator();
  imuInterp->SetInterpolationTypeToLinear();

  // Now, take some anchors timestamp and compute the corresponding
  // anchor sensor pose using the imu trajectory and linearly interpolate
  // the pose between two samples
  std::vector<Eigen::Matrix4d> Hanchors;
  std::vector<int> anchorsIndex;
  int Nanchors = 15;
  for (int i = 1; i < Nanchors + 1; ++i)
  {
    // get tupleId an associated time
    int tupleId = std::floor(static_cast<double>(i * (slamTraj->GetNumberOfPoints() - 1)) / static_cast<double>(Nanchors));
    double time = slamTraj->GetPointData()->GetArray("Time")->GetTuple1(tupleId);
    time += 252018.0000; // time offset between the two clocks

    // Get the anchor associated to this timestamp
    vtkNew<vtkTransform> vtkH;
    imuInterp->InterpolateTransform(time, vtkH.Get());
    vtkNew<vtkMatrix4x4> M;
    vtkH->GetMatrix(M.Get());
    Eigen::Matrix4d currentHanchor;
    for (int col = 0; col < 4; col++)
    {
      for (int raw = 0; raw < 4; ++raw)
      {
        currentHanchor(raw, col) = M->Element[raw][col];
      }
    }
    Hanchors.push_back(currentHanchor);
    anchorsIndex.push_back(tupleId);
  }

  // populate the poses
  PoseEstimationVector absolutePoses;
  ConvertPolyDataToPoseEstimation(slamTraj, absolutePoses);

  // Optimization
  PoseEstimationVector correctedPoses;
  correctedPoses = TrajectoryReoptimization(absolutePoses, Hanchors, anchorsIndex, 1, MeasureProvided::PositionOnly);

  // Convert to vtk
  vtkNew<vtkPolyData> correctedTraj;
  correctedTraj->DeepCopy(slamTraj);
  vtkDataArray* orientation = correctedTraj->GetPointData()->GetArray("Orientation(AxisAngle)");
  for (int poseIndex = 0; poseIndex < slamTraj->GetNumberOfPoints(); ++poseIndex)
  {
    // Orientation
    Eigen::Matrix3d R = correctedPoses[poseIndex].H.block(0, 0, 3, 3);
    Eigen::AngleAxisd angleAxis(R);
    orientation->SetTuple4(poseIndex, angleAxis.axis()(0), angleAxis.axis()(1), angleAxis.axis()(2), angleAxis.angle());

    // Pose
    Eigen::Vector3d T = correctedPoses[poseIndex].H.block(0, 3, 3, 1);
    correctedTraj->GetPoints()->SetPoint(poseIndex, T.data());
  }
}

//----------------------------------------------------------------------------
int TestComputeSimilitude()
{
  std::vector<Eigen::Vector3d> X, Y;
  X.push_back(Eigen::Vector3d(784.151,229.155,35.5491));
  Y.push_back(Eigen::Vector3d(1227,604,100));
  X.push_back(Eigen::Vector3d(958.585,135.759,48.987));
  Y.push_back(Eigen::Vector3d(1432,424,-12));
  X.push_back(Eigen::Vector3d(796.33,-7.14873,51.3654));
  Y.push_back(Eigen::Vector3d(1234,306,14));
  X.push_back(Eigen::Vector3d(381.26,-166.006,37.2953));
  Y.push_back(Eigen::Vector3d(712,104,12.5));
  X.push_back(Eigen::Vector3d(278.693,146.043,13.9819));
  Y.push_back(Eigen::Vector3d(584,497,-1.14));
  X.push_back(Eigen::Vector3d(88.9332,33.0731,4.91454));
  Y.push_back(Eigen::Vector3d(343,356,1.454));

  Eigen::Matrix<double, 9, 1> W = Eigen::Matrix<double, 9, 1>::Zero();
  ComputeSimilitude(X, Y, W);

  Eigen::Matrix<double, 9, 1> Wexpected;
  Wexpected << -0.024524,0.061816,-0.0237559,186.127,248.167,-11.0479,1.25261,1.27832,-1.35793;
  for (int i = 0; i < 9; ++i)
  {
    if (std::abs(W(i) - Wexpected(i)) > 1e-2)
    {
      std::cout << "Got: " << W(i) << " expected: " << Wexpected(i) << std::endl;
      return 1;
    }
  }

  return 0;
}

//----------------------------------------------------------------------------
int main(int argc, char* argv[])
{
  if (argc != 2)
  {
    return 1;
  }

  int errors = 0;
  std::string referenceFilename = std::string(argv[1]) + "/ReferenceCloudFiltered.vtp";
  std::string toAlignedFilename = std::string(argv[1]) + "/ToAlignedCloudFiltered.vtp";

  std::string rawTrajFileName = std::string(argv[1]) + "/TrajectoryWithVarianceCovariance.vtp";
  std::string optimizedTrajFileName = std::string(argv[1]) + "/ExpectedReoptimization.vtp";

  std::string gpsIMUTrajFileName = std::string(argv[1]) + "/Lidar_Calibrated.vtp";

  errors += TestComputeSimilitude();
  errors += TestRelocation(referenceFilename, toAlignedFilename);
  errors += TestTrajectoryReoptimizationLoopClosure(rawTrajFileName, optimizedTrajFileName);
  errors += TestTrajectoryReoptimizationGPSIMU(rawTrajFileName, gpsIMUTrajFileName);

  return errors;
}


// GMTL is (C) Copyright 2001-2010 by Allen Bierbaum
// Distributed under the GNU Lesser General Public License 2.1 with an
// addendum covering inlined code. (See accompanying files LICENSE and
// LICENSE.addendum or http://www.gnu.org/copyleft/lesser.txt)

// Includes ====================================================================
#include <boost/python.hpp>
#include <gmtl-VecOps.h>

// Using =======================================================================
using namespace boost::python;

// Declarations ================================================================

namespace  {

BOOST_PYTHON_FUNCTION_OVERLOADS(isNormalized_overloads_1_2, gmtl::isNormalized, 1, 2)

}// namespace

// Module ======================================================================
void _Export_gmtl_VecOps_h()
{
    def("cross", (gmtl::Vec<double,3> & (*)(gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &))&gmtl::cross, return_internal_reference< 1 >());
    def("cross", (gmtl::Vec<float,3> & (*)(gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &))&gmtl::cross, return_internal_reference< 1 >());
    def("cross", (gmtl::Vec<int,3> & (*)(gmtl::Vec<int,3> &, const gmtl::Vec<int,3> &, const gmtl::Vec<int,3> &))&gmtl::cross, return_internal_reference< 1 >());
    def("dot", (double (*)(const gmtl::VecBase<double,4> &, const gmtl::VecBase<double,4> &))&gmtl::dot);
    def("dot", (double (*)(const gmtl::VecBase<double,3> &, const gmtl::VecBase<double,3> &))&gmtl::dot);
    def("dot", (double (*)(const gmtl::VecBase<double,2> &, const gmtl::VecBase<double,2> &))&gmtl::dot);
    def("dot", (float (*)(const gmtl::VecBase<float,4> &, const gmtl::VecBase<float,4> &))&gmtl::dot);
    def("dot", (float (*)(const gmtl::VecBase<float,3> &, const gmtl::VecBase<float,3> &))&gmtl::dot);
    def("dot", (float (*)(const gmtl::VecBase<float,2> &, const gmtl::VecBase<float,2> &))&gmtl::dot);
    def("dot", (int (*)(const gmtl::VecBase<int,4> &, const gmtl::VecBase<int,4> &))&gmtl::dot);
    def("dot", (int (*)(const gmtl::VecBase<int,3> &, const gmtl::VecBase<int,3> &))&gmtl::dot);
    def("dot", (int (*)(const gmtl::VecBase<int,2> &, const gmtl::VecBase<int,2> &))&gmtl::dot);
    def("isNormalized", (bool (*)(const gmtl::Vec<double,4> &, const double))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<double,3> &, const double))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<double,2> &, const double))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<float,4> &, const float))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<float,3> &, const float))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<float,2> &, const float))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<int,4> &, const int))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<int,3> &, const int))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<int,2> &, const int))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("length", (double (*)(const gmtl::Vec<double,4> &))&gmtl::length);
    def("length", (double (*)(const gmtl::Vec<double,3> &))&gmtl::length);
    def("length", (double (*)(const gmtl::Vec<double,2> &))&gmtl::length);
    def("length", (float (*)(const gmtl::Vec<float,4> &))&gmtl::length);
    def("length", (float (*)(const gmtl::Vec<float,3> &))&gmtl::length);
    def("length", (float (*)(const gmtl::Vec<float,2> &))&gmtl::length);
    def("length", (int (*)(const gmtl::Vec<int,4> &))&gmtl::length);
    def("length", (int (*)(const gmtl::Vec<int,3> &))&gmtl::length);
    def("length", (int (*)(const gmtl::Vec<int,2> &))&gmtl::length);
    def("lengthSquared", (double (*)(const gmtl::Vec<double,4> &))&gmtl::lengthSquared);
    def("lengthSquared", (double (*)(const gmtl::Vec<double,3> &))&gmtl::lengthSquared);
    def("lengthSquared", (double (*)(const gmtl::Vec<double,2> &))&gmtl::lengthSquared);
    def("lengthSquared", (float (*)(const gmtl::Vec<float,4> &))&gmtl::lengthSquared);
    def("lengthSquared", (float (*)(const gmtl::Vec<float,3> &))&gmtl::lengthSquared);
    def("lengthSquared", (float (*)(const gmtl::Vec<float,2> &))&gmtl::lengthSquared);
    def("lengthSquared", (int (*)(const gmtl::Vec<int,4> &))&gmtl::lengthSquared);
    def("lengthSquared", (int (*)(const gmtl::Vec<int,3> &))&gmtl::lengthSquared);
    def("lengthSquared", (int (*)(const gmtl::Vec<int,2> &))&gmtl::lengthSquared);
    def("lerp", (gmtl::VecBase<double,4> & (*)(gmtl::VecBase<double,4> &, const double &, const gmtl::VecBase<double,4> &, const gmtl::VecBase<double,4> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<double,3> & (*)(gmtl::VecBase<double,3> &, const double &, const gmtl::VecBase<double,3> &, const gmtl::VecBase<double,3> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<double,2> & (*)(gmtl::VecBase<double,2> &, const double &, const gmtl::VecBase<double,2> &, const gmtl::VecBase<double,2> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<float,4> & (*)(gmtl::VecBase<float,4> &, const float &, const gmtl::VecBase<float,4> &, const gmtl::VecBase<float,4> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<float,3> & (*)(gmtl::VecBase<float,3> &, const float &, const gmtl::VecBase<float,3> &, const gmtl::VecBase<float,3> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<float,2> & (*)(gmtl::VecBase<float,2> &, const float &, const gmtl::VecBase<float,2> &, const gmtl::VecBase<float,2> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<int,4> & (*)(gmtl::VecBase<int,4> &, const int &, const gmtl::VecBase<int,4> &, const gmtl::VecBase<int,4> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<int,3> & (*)(gmtl::VecBase<int,3> &, const int &, const gmtl::VecBase<int,3> &, const gmtl::VecBase<int,3> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<int,2> & (*)(gmtl::VecBase<int,2> &, const int &, const gmtl::VecBase<int,2> &, const gmtl::VecBase<int,2> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("normalize", (double (*)(gmtl::Vec<double,4> &))&gmtl::normalize);
    def("normalize", (double (*)(gmtl::Vec<double,3> &))&gmtl::normalize);
    def("normalize", (double (*)(gmtl::Vec<double,2> &))&gmtl::normalize);
    def("normalize", (float (*)(gmtl::Vec<float,4> &))&gmtl::normalize);
    def("normalize", (float (*)(gmtl::Vec<float,3> &))&gmtl::normalize);
    def("normalize", (float (*)(gmtl::Vec<float,2> &))&gmtl::normalize);
    def("normalize", (int (*)(gmtl::Vec<int,4> &))&gmtl::normalize);
    def("normalize", (int (*)(gmtl::Vec<int,3> &))&gmtl::normalize);
    def("normalize", (int (*)(gmtl::Vec<int,2> &))&gmtl::normalize);
    def("reflect", (gmtl::VecBase<double,4> & (*)(gmtl::VecBase<double,4> &, const gmtl::VecBase<double,4> &, const gmtl::Vec<double,4> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<double,3> & (*)(gmtl::VecBase<double,3> &, const gmtl::VecBase<double,3> &, const gmtl::Vec<double,3> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<double,2> & (*)(gmtl::VecBase<double,2> &, const gmtl::VecBase<double,2> &, const gmtl::Vec<double,2> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<float,4> & (*)(gmtl::VecBase<float,4> &, const gmtl::VecBase<float,4> &, const gmtl::Vec<float,4> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<float,3> & (*)(gmtl::VecBase<float,3> &, const gmtl::VecBase<float,3> &, const gmtl::Vec<float,3> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<float,2> & (*)(gmtl::VecBase<float,2> &, const gmtl::VecBase<float,2> &, const gmtl::Vec<float,2> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<int,4> & (*)(gmtl::VecBase<int,4> &, const gmtl::VecBase<int,4> &, const gmtl::Vec<int,4> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<int,3> & (*)(gmtl::VecBase<int,3> &, const gmtl::VecBase<int,3> &, const gmtl::Vec<int,3> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<int,2> & (*)(gmtl::VecBase<int,2> &, const gmtl::VecBase<int,2> &, const gmtl::Vec<int,2> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("isEqual", (bool (*)(const gmtl::VecBase<double,4> &, const gmtl::VecBase<double,4> &, const double))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<double,3> &, const gmtl::VecBase<double,3> &, const double))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<double,2> &, const gmtl::VecBase<double,2> &, const double))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<float,4> &, const gmtl::VecBase<float,4> &, const float))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<float,3> &, const gmtl::VecBase<float,3> &, const float))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<float,2> &, const gmtl::VecBase<float,2> &, const float))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<int,4> &, const gmtl::VecBase<int,4> &, const int))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<int,3> &, const gmtl::VecBase<int,3> &, const int))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<int,2> &, const gmtl::VecBase<int,2> &, const int))&gmtl::isEqual);
}


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <util/test_macros.hpp>
#include <boost/range/combine.hpp>
#include <unity/lib/gl_sarray.hpp>
#include <parallel/lambda_omp.hpp>
#include <unity/lib/gl_sframe.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <sframe/sarray.hpp>

using namespace turi;

const flexible_type None = FLEX_UNDEFINED;

struct gl_sarray_test {
  public:
    void test_constructor() {
      gl_sarray sa;
      _assert_sarray_equals(sa, {});

      gl_sarray sa2({1,2,3});
      _assert_sarray_equals(sa2, {1,2,3});
    }

    void test_from_const() {
      gl_sarray sa = gl_sarray::from_const(1, 3);
      _assert_sarray_equals(sa, {1,1,1});
      gl_sarray sb = gl_sarray::from_const("abc", 3);
      _assert_sarray_equals(sb, {"abc","abc","abc"});
      gl_sarray sc = gl_sarray::from_const(FLEX_UNDEFINED, 3);
      _assert_sarray_equals(sc, {FLEX_UNDEFINED, FLEX_UNDEFINED, FLEX_UNDEFINED});
      TS_ASSERT_EQUALS((int)sc.dtype(), (int)flex_type_enum::FLOAT);
    }

    void test_from_sequence() {
      gl_sarray sa = gl_sarray::from_sequence(1,4);
      _assert_sarray_equals(sa, {1,2,3});
    }

    void test_operators() {
      gl_sarray sa({1,2,3});
      gl_sarray ones({1,1,1});
      gl_sarray zeros({0,0,0});

      _assert_sarray_equals(sa + sa, {2,4,6});
      _assert_sarray_equals(sa - sa, {0,0,0});
      _assert_sarray_equals(sa * sa, {1,4,9});
      _assert_sarray_equals(sa / sa, {1,1,1});

      _assert_sarray_equals(sa + 2, {3,4,5});
      _assert_sarray_equals(sa - 2, {-1,0,1});
      _assert_sarray_equals(sa * 2, {2,4,6});
      _assert_sarray_equals(sa / 2.0, {.5,1,1.5});

      _assert_sarray_equals(sa > ones, {0,1,1});
      _assert_sarray_equals(sa < ones, {0,0,0});
      _assert_sarray_equals(sa >= ones, {1,1,1});
      _assert_sarray_equals(sa <= ones, {1,0,0});
      _assert_sarray_equals(sa == ones, {1,0,0});

      _assert_sarray_equals(sa > 1, {0,1,1});
      _assert_sarray_equals(sa < 1, {0,0,0});
      _assert_sarray_equals(sa >= 1, {1,1,1});
      _assert_sarray_equals(sa <= 1, {1,0,0});
      _assert_sarray_equals(sa == 1.0, {1,0,0});

      _assert_sarray_equals(sa & zeros, {0,0,0});
      _assert_sarray_equals(sa && zeros, {0,0,0});
      _assert_sarray_equals(sa | ones, {1,1,1});
      _assert_sarray_equals(sa || ones, {1,1,1});

      gl_sarray tmp(sa);
      _assert_sarray_equals(tmp += sa, {2,4,6}); tmp = sa;
      _assert_sarray_equals(tmp -= sa, {0,0,0}); tmp = sa;
      _assert_sarray_equals(tmp *= sa, {1,4,9}); tmp = sa;
      _assert_sarray_equals(tmp /= sa, {1,1,1}); tmp = sa;

      _assert_sarray_equals(tmp += 2, {3,4,5}); tmp = sa;
      _assert_sarray_equals(tmp -= 2, {-1,0,1}); tmp = sa;
      _assert_sarray_equals(tmp *= 2, {2,4,6}); tmp = sa;
      _assert_sarray_equals(tmp /= 2.0, {.5,1,1.5}); tmp = sa;
    }

    void test_head() {
      gl_sarray sa = gl_sarray::from_sequence(0, 10);
      _assert_sarray_equals(sa.head(5), {0,1,2,3,4});
      _assert_sarray_equals(sa.head(0), {});
      _assert_sarray_equals(sa.head(10), _to_vec(sa));
    }

    void test_tail() {
      gl_sarray sa = gl_sarray::from_sequence(0, 10);
      _assert_sarray_equals(sa.tail(5), {5,6,7,8,9});
      _assert_sarray_equals(sa.tail(0), {});
      _assert_sarray_equals(sa.tail(10), _to_vec(sa));
    }

    void test_astype() {
      gl_sarray sa({1,2,3});
      _assert_sarray_equals(sa.astype(flex_type_enum::FLOAT), {1., 2., 3.});
      _assert_sarray_equals(sa.astype(flex_type_enum::STRING), {"1", "2", "3"});
    }

    void test_sort() {
      gl_sarray sa({4,5,6,1,2,3});
      _assert_sarray_equals(sa.sort(true), {1,2,3,4,5,6});
      _assert_sarray_equals(sa.sort(false), {6,5,4,3,2,1});

      gl_sarray sa_str({"a", "b", "c", "d", "e", "f"});
      _assert_sarray_equals(sa_str.sort(true), {"a", "b", "c", "d", "e", "f"});
    }

    void test_max_min_sum_mean_std() {
      gl_sarray sa({1,2,3,1,2,3});
      TS_ASSERT_EQUALS(sa.min(), 1);
      TS_ASSERT_EQUALS(sa.max(), 3);
      TS_ASSERT_DELTA((double)sa.mean(), 2.0, 1E-6);
      TS_ASSERT_EQUALS(sa.sum(), 12);
      TS_ASSERT(std::abs<double>((double)(sa.std()-std::sqrt(4/6.0))) < 1e-6);
    }

    void test_any_all() {
      TS_ASSERT(gl_sarray({0,0,1}).any());
      TS_ASSERT(!gl_sarray({0,0,0}).any());
      TS_ASSERT(gl_sarray({1,1,1}).all());
      TS_ASSERT(!gl_sarray({0,1,1}).all());
    }

    void test_apply() {
      gl_sarray sa({1,2,3,4,5});
      _assert_sarray_equals(
          sa.apply([](const flexible_type& x) { return x * 2; }, flex_type_enum::INTEGER),
          {2,4,6,8,10});
    }

    void test_filter() {
      gl_sarray sa({1,2,3,4,5});
      _assert_sarray_equals(
          sa.filter([](const flexible_type& x) { return x % 2; }),
          {1,3,5});
    }
    void test_append() {
      auto sa = gl_sarray({1, 2, 3});
      auto sa2 = gl_sarray({4, 5, 6});
      _assert_sarray_equals(sa.append(sa2), {1,2,3,4,5,6});
    }

    void test_unique() {
      gl_sarray sa({1,1,1,2,2,3});
      _assert_sarray_equals(sa.unique().sort(), {1,2,3});
    }

    void test_sample() {
      // This test does not check the sample fraction correctness
      // Even with seed, the answer could be non-deterministic across platform.
      gl_sarray sa({1,1,1,2,2,2,3,3,3,4,4,4});
      TS_ASSERT(sa.sample(0.1, 0).size() < 10);
      _assert_sarray_equals(sa.sample(0.2, 0), _to_vec(sa.sample(0.2,0)));

      auto  sa2 = gl_sarray::from_sequence(0,10);
      std::cout <<  sa2.sample(.3, 12345);
    }

    void test_nnz_num_missing() {
      gl_sarray sa({1,2,3,None,None});
      TS_ASSERT_EQUALS(sa.nnz(), 3);
      TS_ASSERT_EQUALS(sa.num_missing(), 2);
    }

    void test_clip_lower_upper() {
      gl_sarray sa({1,2,3,4,5,6});
      _assert_sarray_equals(sa.clip(3,4), {3,3,3,4,4,4});
      _assert_sarray_equals(sa.clip_lower(3), {3,3,3,4,5,6});
      _assert_sarray_equals(sa.clip_upper(3), {1,2,3,3,3,3});
    }

    void test_dropna_fillna() {
      gl_sarray sa({1,2,3,None,None});
      _assert_sarray_equals(sa.dropna(), {1,2,3});
      _assert_sarray_equals(sa.fillna(0), {1,2,3,0,0});
    }

    void test_topk_index() {
      gl_sarray sa({4,5,6,1,2,3});
      _assert_sarray_equals(sa.topk_index(3), {1,1,1,0,0,0});
    }

    void test_dict_trim_by_keys_values() {
      typedef flex_dict dict;
      std::vector<flexible_type> array;
      array.push_back(dict{ {"A", 65}, {"a",97} });
      array.push_back(dict{ {"B", 66}, {"b",98} });
      array.push_back(dict{ {"C", 67}, {"c",99} });
      gl_sarray sa(array);

      _assert_sarray_equals(
          sa.dict_trim_by_keys({"a", "b","c"}, false), // include
          { dict{{"a",97}}, dict{{"b",98}}, dict{{"c",99}} });

      _assert_sarray_equals(
          sa.dict_trim_by_keys({"a", "b","c"}, true), // exclude
          { dict{{"A",65}}, dict{{"B",66}}, dict{{"C",67}} });

      _assert_sarray_equals(
          sa.dict_trim_by_values(97, 99),
          { dict{{"a",97}}, dict{{"b",98}}, dict{{"c",99}} });
    }

    void test_dict_keys_values() {
      typedef flex_list list;
      typedef flex_dict dict;
      std::vector<flexible_type> array;
      array.push_back(dict{ {"A", 65}, {"a",97} });
      array.push_back(dict{ {"B", 66}, {"b",98} });
      array.push_back(dict{ {"C", 67}, {"c",99} });
      gl_sarray sa(array);

      _assert_sarray_equals(sa.dict_keys(), { list{"A", "a"}, list{"B", "b"}, list{"C", "c"} });
      _assert_sarray_equals(sa.dict_values(), { list{65,97}, list{66,98}, list{67,99} });
    }

    void test_has_any_all_keys() {
      typedef flex_dict dict;
      std::vector<flexible_type> array;
      array.push_back(dict{ {"A", 65}, {"a",97}, {"common", 0} });
      array.push_back(dict{ {"B", 66}, {"b",98}, {"common", 0} });
      array.push_back(dict{ {"C", 67}, {"c",99}, {"common", 1} });
      gl_sarray sa(array);
    }

    void test_count_words() {
      typedef flex_dict dict;
      gl_sarray sa1({ "a", "b,b", "c,c,c"});
      gl_sarray sa2({ "a", "b b", "c c c"});
      _assert_sarray_equals(sa1.count_words(), { dict{{"a",1}}, dict{{"b,b",1}}, dict{{"c,c,c",1}} });
      _assert_sarray_equals(sa2.count_words(), { dict{{"a",1}}, dict{{"b",2}}, dict{{"c",3}} });
    }

    void test_count_ngrams() {
      typedef flex_dict dict;
      gl_sarray sa({ "a", "b,b", "c,c,c"});
      _assert_sarray_equals(sa.count_ngrams(2), { dict{}, dict{{"b b",1}}, dict{{"c c",2}} });
    }

    void test_datetime() {
      boost::posix_time::ptime t(boost::gregorian::date(2011, 1, 1));
      boost::posix_time::ptime epoch(boost::gregorian::date(1970,1,1));
      auto x = (t - epoch).total_seconds();

      auto sa = gl_sarray({flex_date_time(x, 0)});
      std::cout <<  sa.datetime_to_str("%e %b %Y");

      auto  sa2 = gl_sarray({"20-Oct-2011 09:30:10 GMT-05:30"});
      std::cout <<  sa2.str_to_datetime("%d-%b-%Y %H:%M:%S %ZP");
    }

    void test_datetime_to_from_str() {
      typedef flex_date_time date_time;
      gl_sarray sa{ date_time(0, 0), date_time(1, 0), date_time(2, 0) };
      _assert_sarray_equals(sa.datetime_to_str(),
          {"1970-01-01T00:00:00GMT+00", "1970-01-01T00:00:01GMT+00", "1970-01-01T00:00:02GMT+00"});
      _assert_sarray_equals(sa.datetime_to_str().str_to_datetime(),
          _to_vec(sa));
    }

    void test_item_length() {
      auto sa = gl_sarray({flex_dict{{"is_restaurant", 1}, {"is_electronics", 0}},
        flex_dict{{"is_restaurant", 1}, {"is_retail", 1}, {"is_electronics", 0}},
        flex_dict{{"is_restaurant", 0}, {"is_retail", 1}, {"is_electronics", 0}},
        flex_dict{{"is_restaurant", 0}},
        flex_dict{{"is_restaurant", 1}, {"is_electronics", 1}},
        FLEX_UNDEFINED});
      std::cout << sa.item_length();
    }

    void test_split_datetime() {
      // TODO fix datatime split: this test throws exception

      typedef flex_date_time date_time;
      gl_sarray sa{ date_time(0, 0), date_time(1, 0), date_time(2, 0) };
      gl_sframe sf = sa.split_datetime();
      _assert_sarray_equals(sf["X.year"], {1970, 1970, 1970});
      _assert_sarray_equals(sf["X.month"], {1, 1, 1});
      _assert_sarray_equals(sf["X.day"], {1, 1, 1});
      _assert_sarray_equals(sf["X.hour"], {0, 0, 0});
      _assert_sarray_equals(sf["X.minute"], {0, 0, 0});
      _assert_sarray_equals(sf["X.second"], {0, 1, 2});
    }

    void test_split_datetime2() {
      auto sa = gl_sarray({"20-Oct-2011", "10-Jan-2012"});
      auto date_sarray = sa.str_to_datetime("%d-%b-%Y");
      auto split_sf = date_sarray.split_datetime("", {"day","year"});
      std::cout << split_sf;
    }

    void test_unpack() {
      typedef flex_dict dict;
      std::vector<flexible_type> array;
      array.push_back(dict{ {"a", 0}, {"common", 0} });
      array.push_back(dict{ {"b", 1}, {"common", 1} });
      array.push_back(dict{ {"c", 2}, {"common", 2} });
      gl_sarray sa(array);
      gl_sframe sf = sa.unpack("X");
      TS_ASSERT_EQUALS(sf.num_columns(), 4);
      _assert_sarray_equals(sf["X.a"], {0, None, None});
      _assert_sarray_equals(sf["X.b"], {None, 1, None});
      _assert_sarray_equals(sf["X.c"], {None, None, 2});
      _assert_sarray_equals(sf["X.common"], {0, 1, 2});
    }

    void test_unpack2() {
      std::cout << "unpack2:\n\n";
      auto sa = gl_sarray({flex_dict{{"word", "a"},{"count", 1}},
        flex_dict{{"word", "cat"},{"count", 2}},
        flex_dict{{"word", "is"},{"count", 3}},
        flex_dict{{"word", "coming"},{"count", 4}}});
      std::cout <<  sa.unpack("");
      std::cout <<  sa.unpack("X", {}, FLEX_UNDEFINED, {"word"});

      auto  sa2 = gl_sarray({flex_vec{1, 0, 1},
        flex_vec{1, 1, 1},
        flex_vec{0, 1}});
      std::cout <<  sa2.unpack("X", {flex_type_enum::INTEGER,
                flex_type_enum::INTEGER,
                flex_type_enum::INTEGER}, 0);
    }

    void test_basic_indexing_and_ranges() {
      turi::gl_sarray a{1,2,3,4,5,6,7,8,9,10};
      a += 1;
      auto t = a[a > 2 && a <= 8];

      std::cout << a << "\n"
                << t << "\n";

      t = t + 1;

      turi::gl_sarray expected{4,5,6,7,8,9};

      // indexing teset
      for (size_t i = 0; i < t.size(); ++i) {
        TS_ASSERT_EQUALS(t[i], expected[i]);
      }

      size_t ctr = 2;
      for(const auto& vals : a.range_iterator()) {
        TS_ASSERT_EQUALS((int)vals, ctr);
        ++ctr;
      }
      TS_ASSERT_EQUALS(ctr, 12);

      // parallel range iterator test
      turi::gl_sarray src = gl_sarray::from_const(0, 1000);
      size_t src_size = src.size();
      in_parallel([&](size_t thread_idx, size_t num_threads) {
        size_t start_idx = src_size * thread_idx / num_threads;
        size_t end_idx = src_size * (thread_idx + 1) / num_threads;
        for (const auto& v: src.range_iterator(start_idx, end_idx)) {
          TS_ASSERT_EQUALS((int)v, 0);
        }
      });

      // range iterator test
      TS_ASSERT_EQUALS(t.size(), expected.size());
      auto range1 = t.range_iterator();
      auto range2 = expected.range_iterator();
      auto start1 = range1.begin();
      auto start2 = range2.begin();
      while(start1 != range1.end()) {
        TS_ASSERT_EQUALS((*start1), (*start2));
        ++start1;
        ++start2;
      }

      auto b = a.sort(false)[{1, 8}].sort();
      std::cout << b;
      for (size_t i = 0; i < t.size(); ++i) {
        TS_ASSERT_EQUALS(b[i], expected[i]);
      }
    }

    void test_lazy_iteration() {
      gl_sarray sa = gl_sarray::from_const(0, 1000) + 1;
      atomic<size_t> ctr;
      sa.materialize_to_callback([&](size_t thrid,
                                    const std::shared_ptr<sframe_rows>& rows) {
                                      ctr += rows->num_rows();
                                      return false;
                                    });
      TS_ASSERT_EQUALS(ctr.value, 1000);
      TS_ASSERT_EQUALS(sa.is_materialized(), false);
    }

    void test_writer() {
      gl_sarray_writer writer(flex_type_enum::INTEGER);
      // write one integer into each segment, so we get [0, 1, 2, 3 ... #segments - 1]
      // then write a few more integers to the end
      for (size_t i = 0;i < writer.num_segments(); ++i) {
        writer.write(i, i);
      }
      std::vector<size_t> values;
      for (size_t i = writer.num_segments(); i < 100; ++i) values.push_back(i);
      writer.write(values.begin(), values.end(), writer.num_segments() - 1);
      gl_sarray array = writer.close();

      // done!
      gl_sarray range_values = gl_sarray::from_sequence(0, 100);
      _assert_sarray_equals(array, _to_vec(range_values));
    }

    void test_slice() {
      gl_sarray a{1,2,3,4,5,6,7,8,9,10};

      std::cout << "\n" << a[{1,4}];  // start at index 1, end at index 4
      // ret is the array [2,3,4]
      std::cout << "\n" << a[{1,2,8}];  // start at index 1, end at index 8 with step size 2
      // ret is the array [2,4,6,8]

      std::cout << "\n" << a[{-3,-1}];  // start at end - 3, end at index end - 1
      // ret is the array [8,9]
    }

    void test_rolling_apply() {
      gl_sarray a{0,1,2,3,4,5,6,7,8,9};
      auto result = a.builtin_rolling_apply(std::string("__builtin__avg__"), -3, 0);
      _assert_sarray_equals(result,{flex_undefined(),
        flex_undefined(),flex_undefined(),1.5,2.5,3.5,4.5,5.5,6.5,7.5});
    }

    void test_sarray() {
      gl_sarray sa{1,2,3,4,5,6};

      auto sa2 = sa.materialize_to_sarray();
      gl_sarray sa3 = sa2;

      _assert_sarray_equals(sa, _to_vec(sa3));
    }

    void test_cumulative_sum() {
      auto single_test = [&](const gl_sarray& in, const gl_sarray& ans) {
        gl_sarray out = in.cumulative_sum();
        //for (size_t i = 0; i < out.size(); i++) {
        //  std::cout << " i   = " << i
        //            << " in  = " << in[i]
        //            << " out = " << out[i]
        //            << " ans = " << ans[i] << std::endl;
        //}
        _assert_sarray_equals(out, _to_vec(ans));
      };

      single_test(
          gl_sarray{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10},
          gl_sarray{0, 1, 3, 6, 10, 15, 21, 28, 36, 45, 55}
      );
      single_test(
          gl_sarray{0.1, 1.1, 2.1, 3.1, 4.1, 5.1, 6.1, 7.1},
          gl_sarray{0.1, 1.2, 3.3, 6.4, 10.5, 15.6, 21.7, 28.8}
      );
      single_test(
          gl_sarray{{11.0, 2.0}, {22.0, 1.0}, {3.0, 4.0}, {4.0, 4.0}},
          gl_sarray{{11.0, 2.0}, {33.0, 3.0}, {36.0, 7.0}, {40.0, 11.0}}
      );
      single_test(
          gl_sarray{FLEX_UNDEFINED, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10},
          gl_sarray{FLEX_UNDEFINED, 1, 3, 6, 10, 15, 21, 28, 36, 45, 55}
      );
      single_test(
          gl_sarray{FLEX_UNDEFINED, 1, FLEX_UNDEFINED, 3, FLEX_UNDEFINED, 5},
          gl_sarray{FLEX_UNDEFINED, 1, 1, 4, 4, 9}
      );
      single_test(
          gl_sarray{{33.0, 3.0}, FLEX_UNDEFINED, {3.0, 4.0}, {4.0, 4.0}},
          gl_sarray({{33.0, 3.0}, {33.0, 3.0}, {36.0, 7.0}, {40.0, 11.0}}, flex_type_enum::VECTOR)
      );
      single_test(
          gl_sarray{{33.0, 3.0}, FLEX_UNDEFINED, FLEX_UNDEFINED, {4.0, 4.0}},
          gl_sarray({{33.0, 3.0}, {33.0, 3.0}, {33.0, 3.0}, {37.0, 7.0}}, flex_type_enum::VECTOR)
      );

    }

    void test_cumulative_avg() {
      auto single_test = [&](const gl_sarray& in, const gl_sarray& ans) {
        gl_sarray out = in.cumulative_avg();
        _assert_sarray_equals(out, _to_vec(ans));
      };

      single_test(
          gl_sarray{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10},
          gl_sarray{0.0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5, 5.0}
      );
      single_test(
          gl_sarray{0.1, 1.1, 2.1, 3.1, 4.1, 5.1, 6.1, 7.1},
          gl_sarray{0.1, 0.6, 1.1, 1.6, 2.1, 2.6, 3.1, 3.6}
      );
      single_test(
          gl_sarray{{11.0, 22.0}, {33.0, 66.0}, {4.0,   2.0}, {4.0,  2.0}},
          gl_sarray{{11.0, 22.0}, {22.0, 44.0}, {16.0, 30.0}, {13.0, 23.0}}
      );
    }

    void test_cumulative_min() {
      auto single_test = [&](const gl_sarray& in, const gl_sarray& ans) {
        gl_sarray out = in.cumulative_min();
        _assert_sarray_equals(out, _to_vec(ans));
      };

      single_test(
          gl_sarray{0, 1, 2, 3, 4, 5, -1, 7, 8, -2, 10},
          gl_sarray{0, 0, 0, 0, 0, 0, -1, -1, -1, -2, -2}
      );
      single_test(
          gl_sarray{7.1, 6.1, 3.1, 3.9, 4.1, 2.1, 2.9, 0.1},
          gl_sarray{7.1, 6.1, 3.1, 3.1, 3.1, 2.1, 2.1, 0.1}
      );
    }

    void test_cumulative_max() {
      auto single_test = [&](const gl_sarray& in, const gl_sarray& ans) {
        gl_sarray out = in.cumulative_max();
        _assert_sarray_equals(out, _to_vec(ans));
      };

      single_test(
          gl_sarray{0, 1, 0, 3, 5, 4, 1, 7, 6, 2, 10},
          gl_sarray{0, 1, 1, 3, 5, 5, 5, 7, 7, 7, 10}
      );
      single_test(
          gl_sarray{2.1, 6.1, 3.1, 3.9, 2.1, 8.1, 8.9, 10.1},
          gl_sarray{2.1, 6.1, 6.1, 6.1, 6.1, 8.1, 8.9, 10.1}
      );
    }

    std::vector<flexible_type> _to_vec(gl_sarray sa) {
      std::vector<flexible_type> ret;
      for (auto& v: sa.range_iterator()) { ret.push_back(v); }
      return ret;
    }

    void _assert_sarray_equals(gl_sarray sa, const std::vector<flexible_type>& vec) {
      TS_ASSERT_EQUALS(sa.size(), vec.size());
      for (size_t i = 0; i < vec.size(); ++i) {
        if (sa[i].get_type() == flex_type_enum::FLOAT) {
          TS_ASSERT_DELTA(sa[i], vec[i], 1e-12);
        } else {
          TS_ASSERT_EQUALS(sa[i], vec[i]);
        }
      }
    }
};

BOOST_FIXTURE_TEST_SUITE(_gl_sarray_test, gl_sarray_test)
BOOST_AUTO_TEST_CASE(test_constructor) {
  gl_sarray_test::test_constructor();
}
BOOST_AUTO_TEST_CASE(test_from_const) {
  gl_sarray_test::test_from_const();
}
BOOST_AUTO_TEST_CASE(test_from_sequence) {
  gl_sarray_test::test_from_sequence();
}
BOOST_AUTO_TEST_CASE(test_operators) {
  gl_sarray_test::test_operators();
}
BOOST_AUTO_TEST_CASE(test_head) {
  gl_sarray_test::test_head();
}
BOOST_AUTO_TEST_CASE(test_tail) {
  gl_sarray_test::test_tail();
}
BOOST_AUTO_TEST_CASE(test_astype) {
  gl_sarray_test::test_astype();
}
BOOST_AUTO_TEST_CASE(test_sort) {
  gl_sarray_test::test_sort();
}
BOOST_AUTO_TEST_CASE(test_max_min_sum_mean_std) {
  gl_sarray_test::test_max_min_sum_mean_std();
}
BOOST_AUTO_TEST_CASE(test_any_all) {
  gl_sarray_test::test_any_all();
}
BOOST_AUTO_TEST_CASE(test_apply) {
  gl_sarray_test::test_apply();
}
BOOST_AUTO_TEST_CASE(test_filter) {
  gl_sarray_test::test_filter();
}
BOOST_AUTO_TEST_CASE(test_append) {
  gl_sarray_test::test_append();
}
BOOST_AUTO_TEST_CASE(test_unique) {
  gl_sarray_test::test_unique();
}
BOOST_AUTO_TEST_CASE(test_sample) {
  gl_sarray_test::test_sample();
}
BOOST_AUTO_TEST_CASE(test_nnz_num_missing) {
  gl_sarray_test::test_nnz_num_missing();
}
BOOST_AUTO_TEST_CASE(test_clip_lower_upper) {
  gl_sarray_test::test_clip_lower_upper();
}
BOOST_AUTO_TEST_CASE(test_dropna_fillna) {
  gl_sarray_test::test_dropna_fillna();
}
BOOST_AUTO_TEST_CASE(test_topk_index) {
  gl_sarray_test::test_topk_index();
}
BOOST_AUTO_TEST_CASE(test_dict_trim_by_keys_values) {
  gl_sarray_test::test_dict_trim_by_keys_values();
}
BOOST_AUTO_TEST_CASE(test_dict_keys_values) {
  gl_sarray_test::test_dict_keys_values();
}
BOOST_AUTO_TEST_CASE(test_has_any_all_keys) {
  gl_sarray_test::test_has_any_all_keys();
}
BOOST_AUTO_TEST_CASE(test_count_words) {
  gl_sarray_test::test_count_words();
}
BOOST_AUTO_TEST_CASE(test_count_ngrams) {
  gl_sarray_test::test_count_ngrams();
}
BOOST_AUTO_TEST_CASE(test_datetime) {
  gl_sarray_test::test_datetime();
}
BOOST_AUTO_TEST_CASE(test_datetime_to_from_str) {
  gl_sarray_test::test_datetime_to_from_str();
}
BOOST_AUTO_TEST_CASE(test_item_length) {
  gl_sarray_test::test_item_length();
}
BOOST_AUTO_TEST_CASE(test_split_datetime) {
  gl_sarray_test::test_split_datetime();
}
BOOST_AUTO_TEST_CASE(test_split_datetime2) {
  gl_sarray_test::test_split_datetime2();
}
BOOST_AUTO_TEST_CASE(test_unpack) {
  gl_sarray_test::test_unpack();
}
BOOST_AUTO_TEST_CASE(test_unpack2) {
  gl_sarray_test::test_unpack2();
}
BOOST_AUTO_TEST_CASE(test_basic_indexing_and_ranges) {
  gl_sarray_test::test_basic_indexing_and_ranges();
}
BOOST_AUTO_TEST_CASE(test_lazy_iteration) {
  gl_sarray_test::test_lazy_iteration();
}
BOOST_AUTO_TEST_CASE(test_writer) {
  gl_sarray_test::test_writer();
}
BOOST_AUTO_TEST_CASE(test_slice) {
  gl_sarray_test::test_slice();
}
BOOST_AUTO_TEST_CASE(test_rolling_apply) {
  gl_sarray_test::test_rolling_apply();
}
BOOST_AUTO_TEST_CASE(test_sarray) {
  gl_sarray_test::test_sarray();
}
BOOST_AUTO_TEST_CASE(test_cumulative_sum) {
  gl_sarray_test::test_cumulative_sum();
}
BOOST_AUTO_TEST_CASE(test_cumulative_avg) {
  gl_sarray_test::test_cumulative_avg();
}
BOOST_AUTO_TEST_CASE(test_cumulative_min) {
  gl_sarray_test::test_cumulative_min();
}
BOOST_AUTO_TEST_CASE(test_cumulative_max) {
  gl_sarray_test::test_cumulative_max();
}
BOOST_AUTO_TEST_SUITE_END()


//
// Copyright (c) 2009 Rutger ter Borg
//
// 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_NUMERIC_BINDINGS_UBLAS_MATRIX_SPARSE_HPP
#define BOOST_NUMERIC_BINDINGS_UBLAS_MATRIX_SPARSE_HPP

#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/end.hpp>
#include <boost/numeric/bindings/detail/adaptor.hpp>
#include <boost/numeric/bindings/detail/copy_const.hpp>
#include <boost/numeric/bindings/ublas/detail/convert_to.hpp>
#include <boost/numeric/bindings/ublas/matrix_expression.hpp>
#include <boost/numeric/bindings/ublas/storage.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace detail {

template< typename T, typename F, std::size_t IB, typename IA, typename TA, typename Id, typename Enable >
struct adaptor< ublas::compressed_matrix< T, F, IB, IA, TA >, Id, Enable > {

    typedef typename copy_const< Id, T >::type value_type;
    typedef typename copy_const< Id, typename bindings::value_type<IA>::type >::type index_type;
    typedef typename convert_to< tag::data_order, F >::type data_order;
    typedef mpl::map<
        mpl::pair< tag::value_type, value_type >,
        mpl::pair< tag::index_type, index_type >,
        mpl::pair< tag::entity, tag::matrix >,
        mpl::pair< tag::size_type<1>, std::ptrdiff_t >,
        mpl::pair< tag::size_type<2>, std::ptrdiff_t >,
        mpl::pair< tag::matrix_type, tag::general >,
        mpl::pair< tag::data_structure, tag::compressed_sparse >,
        mpl::pair< tag::data_order, data_order >,
        mpl::pair< tag::index_base, mpl::int_<IB> >
    > property_map;

    static std::ptrdiff_t size1( const Id& id ) {
        return id.size1();
    }

    static std::ptrdiff_t size2( const Id& id ) {
        return id.size2();
    }

    static value_type* begin_value( Id& id ) {
        return bindings::begin_value( id.value_data() );
    }

    static value_type* end_value( Id& id ) {
        return bindings::begin_value( id.value_data() ) + id.nnz();
    }

    static index_type* begin_compressed_index_major( Id& id ) {
        return bindings::begin_value( id.index1_data() );
    }

    static index_type* end_compressed_index_major( Id& id ) {
        return bindings::end_value( id.index1_data() );
    }

    static index_type* begin_index_minor( Id& id ) {
        return bindings::begin_value( id.index2_data() );
    }

    static index_type* end_index_minor( Id& id ) {
        return bindings::begin_value( id.index2_data() ) + id.nnz();
    }

};

template< typename T, typename F, std::size_t IB, typename IA, typename TA, typename Id, typename Enable >
struct adaptor< ublas::coordinate_matrix< T, F, IB, IA, TA >, Id, Enable > {

    typedef typename copy_const< Id, T >::type value_type;
    typedef typename copy_const< Id, typename bindings::value_type<IA>::type >::type index_type;
    typedef typename convert_to< tag::data_order, F >::type data_order;
    typedef mpl::map<
        mpl::pair< tag::value_type, value_type >,
        mpl::pair< tag::index_type, index_type >,
        mpl::pair< tag::entity, tag::matrix >,
        mpl::pair< tag::size_type<1>, std::ptrdiff_t >,
        mpl::pair< tag::size_type<2>, std::ptrdiff_t >,
        mpl::pair< tag::matrix_type, tag::general >,
        mpl::pair< tag::data_structure, tag::coordinate_sparse >,
        mpl::pair< tag::data_order, data_order >,
        mpl::pair< tag::index_base, mpl::int_<IB> >
    > property_map;

    static std::ptrdiff_t size1( const Id& id ) {
        return id.size1();
    }

    static std::ptrdiff_t size2( const Id& id ) {
        return id.size2();
    }

    static value_type* begin_value( Id& id ) {
        return bindings::begin_value( id.value_data() );
    }

    static value_type* end_value( Id& id ) {
        return bindings::begin_value( id.value_data() ) + id.nnz();
    }

    static index_type* begin_index_major( Id& id ) {
        return bindings::begin_value( id.index1_data() );
    }

    static index_type* end_index_major( Id& id ) {
        return bindings::begin_value( id.index1_data() ) + id.nnz();
    }

    static index_type* begin_index_minor( Id& id ) {
        return bindings::begin_value( id.index2_data() );
    }

    static index_type* end_index_minor( Id& id ) {
        return bindings::begin_value( id.index2_data() ) + id.nnz();
    }

};

} // namespace detail
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif