// Copyright (C) 2012  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/graph_cuts.h>
#include <dlib/graph_utils.h>
#include <dlib/directed_graph.h>
#include <dlib/graph.h>
#include <dlib/rand.h>
#include <dlib/hash.h>
#include <dlib/image_transforms.h>

#include "tester.h"

namespace
{

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

    logger dlog("test.graph_cuts");

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

    class dense_potts_problem
    {
    public:
        typedef double value_type;
    private:

        matrix<value_type,0,1> factors1;
        matrix<value_type> factors2;
        matrix<node_label,0,1> labels;
    public:

        dense_potts_problem (
            unsigned long num_nodes,
            dlib::rand& rnd
        )
        {
            factors1 = -7*(randm(num_nodes, 1, rnd)-0.5);
            factors2 = make_symmetric(randm(num_nodes, num_nodes, rnd) > 0.5);
            labels.set_size(num_nodes);
            labels = FREE_NODE;
        }

        unsigned long number_of_nodes (
        ) const { return factors1.nr(); }

        unsigned long number_of_neighbors (
            unsigned long // idx
        ) const { return number_of_nodes()-1; }

        unsigned long get_neighbor_idx (
            unsigned long node_id1,
            unsigned long node_id2
        ) const
        {
            if (node_id2 < node_id1)
                return node_id2;
            else
                return node_id2-1;
        }

        unsigned long get_neighbor (
            unsigned long node_id,
            unsigned long idx
        ) const
        {
            DLIB_TEST(node_id < number_of_nodes());
            DLIB_TEST(idx < number_of_neighbors(node_id));
            if (idx < node_id)
                return idx;
            else
                return idx+1;
        }

        void set_label (
            const unsigned long& idx,
            node_label value
        )
        {
            labels(idx) = value;
        }

        node_label get_label (
            const unsigned long& idx
        ) const
        {
            return labels(idx);
        }

        value_type factor_value (unsigned long idx) const
        {
            DLIB_TEST(idx < number_of_nodes());

            return factors1(idx);
        }

        value_type factor_value_disagreement (unsigned long idx1, unsigned long idx2) const
        {
            DLIB_TEST(idx1 != idx2);
            DLIB_TEST(idx1 < number_of_nodes());
            DLIB_TEST(idx2 < number_of_nodes());
            DLIB_TEST(get_neighbor_idx(idx1,idx2) < number_of_neighbors(idx1));
            DLIB_TEST(get_neighbor_idx(idx2,idx1) < number_of_neighbors(idx2));

            return factors2(idx1, idx2);
        }

    };

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

    class image_potts_problem
    {
    public:
        typedef double value_type;
        const static unsigned long max_number_of_neighbors = 4;
    private:

        matrix<value_type,0,1> factors1;
        matrix<value_type> factors2;
        matrix<node_label,0,1> labels;
        long nr;
        long nc;
        rectangle rect, inner_rect;
        mutable long count;
    public:

        image_potts_problem (
            long nr_,
            long nc_,
            dlib::rand& rnd
        ) : nr(nr_), nc(nc_)
        {
            rect = rectangle(0,0,nc-1,nr-1);
            inner_rect = shrink_rect(rect,1);
            const unsigned long num_nodes = nr*nc;
            factors1 = -7*(randm(num_nodes, 1, rnd));
            factors2 = randm(num_nodes, 4, rnd) > 0.5;

            //factors1 = 0;
            //set_rowm(factors1, range(0, factors1.nr()/2)) = -1;

            labels.set_size(num_nodes);
            labels = FREE_NODE;

            count = 0;
        }

        ~image_potts_problem()
        {
            dlog << LTRACE << "interface calls: " << count;
            dlog << LTRACE << "labels hash: "<< murmur_hash3_128bit(&labels(0), labels.size()*sizeof(labels(0)), 0).first;
        }

        unsigned long number_of_nodes (
        ) const { return factors1.nr(); }

        unsigned long number_of_neighbors (
            unsigned long idx
        ) const
        {
            ++count;
            const point& p = get_loc(idx);
            if (inner_rect.contains(p))
                return 4;
            else if (p == rect.tl_corner() ||
                     p == rect.bl_corner() ||
                     p == rect.tr_corner() ||
                     p == rect.br_corner() )
                return 2;
            else
                return 3;
        }

        unsigned long get_neighbor_idx (
            long node_id1,
            long node_id2
        ) const
        {
            ++count;
            const point& p = get_loc(node_id1);
            long ret = 0;
            if (rect.contains(p + point(1,0)))
            {
                if (node_id2-node_id1 == 1)
                    return ret;
                ++ret;
            }

            if (rect.contains(p - point(1,0)))
            {
                if (node_id2-node_id1 == -1)
                    return ret;
                ++ret;
            }

            if (rect.contains(p + point(0,1)))
            {
                if (node_id2-node_id1 == nc)
                    return ret;
                ++ret;
            }

            return ret;
        }

        unsigned long get_neighbor (
            long node_id,
            long idx
        ) const
        {
            ++count;
            const point& p = get_loc(node_id);
            if (rect.contains(p + point(1,0)))
            {
                if (idx == 0)
                    return node_id+1;
                --idx;
            }

            if (rect.contains(p - point(1,0)))
            {
                if (idx == 0)
                    return node_id-1;
                --idx;
            }

            if (rect.contains(p + point(0,1)))
            {
                if (idx == 0)
                    return node_id+nc;
                --idx;
            }

            return node_id-nc;
        }

        void set_label (
            const unsigned long& idx,
            node_label value
        )
        {
            ++count;
            labels(idx) = value;
        }

        node_label get_label (
            const unsigned long& idx
        ) const
        {
            ++count;
            return labels(idx);
        }

        value_type factor_value (unsigned long idx) const
        {
            ++count;
            DLIB_TEST(idx < (unsigned long)number_of_nodes());

            return factors1(idx);
        }

        value_type factor_value_disagreement (unsigned long idx1, unsigned long idx2) const
        {
            ++count;
            DLIB_TEST(idx1 != idx2);
            DLIB_TEST(idx1 < (unsigned long)number_of_nodes());
            DLIB_TEST(idx2 < (unsigned long)number_of_nodes());

            // make this function symmetric
            if (idx1 > idx2)
                swap(idx1,idx2);

            DLIB_TEST(get_neighbor(idx1, get_neighbor_idx(idx1, idx2)) == idx2);
            DLIB_TEST(get_neighbor(idx2, get_neighbor_idx(idx2, idx1)) == idx1);

            // the neighbor relationship better be symmetric
            DLIB_TEST(get_neighbor_idx(idx1,idx2) < number_of_neighbors(idx1));
            DLIB_TEST_MSG(get_neighbor_idx(idx2,idx1) < number_of_neighbors(idx2),
                         "\n idx1: "<< idx1  <<
                         "\n idx2: "<< idx2  <<
                         "\n get_neighbor_idx(idx2,idx1): "<< get_neighbor_idx(idx2,idx1) <<
                         "\n number_of_neighbors(idx2): " << number_of_neighbors(idx2) <<
                         "\n nr: "<< nr <<
                         "\n nc: "<< nc
            );

            return factors2(idx1, get_neighbor_idx(idx1,idx2));
        }

    private:
        point get_loc (
            const unsigned long& idx
        ) const
        {
            return point(idx%nc, idx/nc);
        }

    };

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

    template <typename potts_model>
    void brute_force_potts_model (
        potts_model& g
    )
    {
        potts_model m(g);

        const unsigned long num = (unsigned long)std::pow(2.0, (double)m.number_of_nodes());

        double best_score = -std::numeric_limits<double>::infinity();
        for (unsigned long i = 0; i < num; ++i)
        {
            for (unsigned long j = 0; j < m.number_of_nodes(); ++j)
            {
                unsigned long T = (1)<<j;
                T = (T&i);
                if (T != 0)
                    m.set_label(j,SINK_CUT);
                else
                    m.set_label(j,SOURCE_CUT);
            }

            double score = potts_model_score(m);
            if (score > best_score)
            {
                best_score = score;
                g = m;
            }
        }
    }

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

    template <typename graph_type>
    void brute_force_potts_model_on_graph (
        const graph_type& g,
        std::vector<node_label>& labels_
    )
    {
        std::vector<node_label> labels;
        labels.resize(g.number_of_nodes());

        const unsigned long num = (unsigned long)std::pow(2.0, (double)g.number_of_nodes());

        double best_score = -std::numeric_limits<double>::infinity();
        for (unsigned long i = 0; i < num; ++i)
        {
            for (unsigned long j = 0; j < g.number_of_nodes(); ++j)
            {
                unsigned long T = (1)<<j;
                T = (T&i);
                if (T != 0)
                    labels[j] = SINK_CUT;
                else
                    labels[j] = SOURCE_CUT;
            }

            double score = potts_model_score(g,labels);
            if (score > best_score)
            {
                best_score = score;
                labels_ = labels;
            }
        }
    }

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

    template <typename graph_type>
    void make_random_undirected_graph(
        dlib::rand& rnd,
        graph_type& g
    )
    {
        typedef typename graph_type::edge_type edge_weight_type;
        g.clear();
        const unsigned int num_nodes = rnd.get_random_32bit_number()%8;
        g.set_number_of_nodes(num_nodes);

        const unsigned int num_edges = static_cast<unsigned int>(num_nodes*(num_nodes-1)/2*rnd.get_random_double() + 0.5);

        // add the right number of randomly selected edges
        unsigned int count = 0;
        while (count < num_edges)
        {
            unsigned long i = rnd.get_random_32bit_number()%g.number_of_nodes();
            unsigned long j = rnd.get_random_32bit_number()%g.number_of_nodes();
            if (i != j && g.has_edge(i, j) == false)
            {
                ++count;
                g.add_edge(i, j);
                edge(g, i, j) = static_cast<edge_weight_type>(rnd.get_random_double()*50);
            }
        }

        for (unsigned long i = 0; i < g.number_of_nodes(); ++i)
        {
            g.node(i).data = static_cast<edge_weight_type>(rnd.get_random_gaussian()*200);
        }
    }

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

    void test_graph_potts_model(
        dlib::rand& rnd
    )
    {
        using namespace std;
        double brute_force_score;
        double graph_cut_score;

        graph<double,double>::kernel_1a_c temp;
        make_random_undirected_graph(rnd,temp);

        {
            std::vector<node_label> labels;

            brute_force_potts_model_on_graph(temp, labels);

            for (unsigned long i = 0; i < temp.number_of_nodes(); ++i)
            {
                dlog << LTRACE << "node " << i << ": "<< (int)labels[i];
            }

            brute_force_score = potts_model_score(temp, labels);
            dlog << LTRACE << "brute force score: "<< brute_force_score;
        }
        dlog << LTRACE << "******************";

        {
            std::vector<node_label> labels;
            find_max_factor_graph_potts(temp, labels);
            DLIB_TEST(temp.number_of_nodes() == labels.size());

            for (unsigned long i = 0; i < temp.number_of_nodes(); ++i)
            {
                dlog << LTRACE << "node " << i << ": "<< (int)labels[i];
            }
            graph_cut_score = potts_model_score(temp, labels);
            dlog << LTRACE << "graph cut score: "<< graph_cut_score;
        }

        DLIB_TEST_MSG(graph_cut_score == brute_force_score, std::abs(graph_cut_score - brute_force_score));

        dlog << LTRACE << "##################";
        dlog << LTRACE << "##################";
        dlog << LTRACE << "##################";
    }

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

    template <typename potts_prob>
    void impl_test_potts_model (
        potts_prob& p
    )
    {
        using namespace std;
        double brute_force_score;
        double graph_cut_score;

        {
            potts_prob temp(p);
            brute_force_potts_model(temp);

            for (unsigned long i = 0; i < temp.number_of_nodes(); ++i)
            {
                dlog << LTRACE << "node " << i << ": "<< (int)temp.get_label(i);
            }
            brute_force_score = potts_model_score(temp);
            dlog << LTRACE << "brute force score: "<< brute_force_score;
        }
        dlog << LTRACE << "******************";

        {
            potts_prob temp(p);
            find_max_factor_graph_potts(temp);

            for (unsigned long i = 0; i < temp.number_of_nodes(); ++i)
            {
                dlog << LTRACE << "node " << i << ": "<< (int)temp.get_label(i);
            }
            graph_cut_score = potts_model_score(temp);
            dlog << LTRACE << "graph cut score: "<< graph_cut_score;
        }

        DLIB_TEST_MSG(graph_cut_score == brute_force_score, std::abs(graph_cut_score - brute_force_score));

        dlog << LTRACE << "##################";
        dlog << LTRACE << "##################";
        dlog << LTRACE << "##################";
    }

// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
//                                   BASIC MIN CUT STUFF
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------

    template <typename directed_graph>
    void brute_force_min_cut (
        directed_graph& g,
        unsigned long source,
        unsigned long sink
    )
    {
        typedef typename directed_graph::edge_type edge_weight_type;
        const unsigned long num = (unsigned long)std::pow(2.0, (double)g.number_of_nodes());

        std::vector<node_label> best_cut(g.number_of_nodes(),FREE_NODE);

        edge_weight_type best_score = std::numeric_limits<edge_weight_type>::max();
        for (unsigned long i = 0; i < num; ++i)
        {
            for (unsigned long j = 0; j < g.number_of_nodes(); ++j)
            {
                unsigned long T = (1)<<j;
                T = (T&i);
                if (T != 0)
                    g.node(j).data = SINK_CUT;
                else
                    g.node(j).data = SOURCE_CUT;
            }

            // ignore cuts that don't label the source or sink node the way we want.
            if (g.node(source).data != SOURCE_CUT ||
                g.node(sink).data != SINK_CUT)
                continue;

            edge_weight_type score = graph_cut_score(g);
            if (score < best_score)
            {
                best_score = score;
                for (unsigned long j = 0; j < g.number_of_nodes(); ++j)
                    best_cut[j] = g.node(j).data;
            }
        }

        for (unsigned long j = 0; j < g.number_of_nodes(); ++j)
            g.node(j).data =  best_cut[j];
    }

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

    template <typename directed_graph>
    void print_graph(
        const directed_graph& g
    )
    {
        using namespace std;
        dlog << LTRACE << "number of nodes: "<< g.number_of_nodes();
        for (unsigned long i = 0; i < g.number_of_nodes(); ++i)
        {
            for (unsigned long n = 0; n < g.node(i).number_of_children(); ++n)
                dlog << LTRACE << i << " -(" << g.node(i).child_edge(n) << ")-> " << g.node(i).child(n).index();
        }
    }

    template <typename directed_graph>
    void copy_edge_weights (
        directed_graph& dest,
        const directed_graph& src
    )
    {
        for (unsigned long i = 0; i < src.number_of_nodes(); ++i)
        {
            for (unsigned long n = 0; n < src.node(i).number_of_children(); ++n)
            {
                dest.node(i).child_edge(n) = src.node(i).child_edge(n);
            }
        }
    }

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

    template <typename graph_type>
    void pick_random_source_and_sink (
        dlib::rand& rnd,
        const graph_type& g,
        unsigned long& source,
        unsigned long& sink
    )
    {
        source = rnd.get_random_32bit_number()%g.number_of_nodes();
        sink = rnd.get_random_32bit_number()%g.number_of_nodes();
        while (sink == source)
            sink = rnd.get_random_32bit_number()%g.number_of_nodes();
    }

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

    template <typename dgraph_type>
    void make_random_graph(
        dlib::rand& rnd,
        dgraph_type& g,
        unsigned long& source,
        unsigned long& sink
    )
    {
        typedef typename dgraph_type::edge_type edge_weight_type;
        g.clear();
        const unsigned int num_nodes = rnd.get_random_32bit_number()%7 + 2;
        g.set_number_of_nodes(num_nodes);

        const unsigned int num_edges = static_cast<unsigned int>(num_nodes*(num_nodes-1)/2*rnd.get_random_double() + 0.5);

        // add the right number of randomly selected edges
        unsigned int count = 0;
        while (count < num_edges)
        {
            unsigned long parent = rnd.get_random_32bit_number()%g.number_of_nodes();
            unsigned long child = rnd.get_random_32bit_number()%g.number_of_nodes();
            if (parent != child && g.has_edge(parent, child) == false)
            {
                ++count;
                g.add_edge(parent, child);
                edge(g, parent, child) = static_cast<edge_weight_type>(rnd.get_random_double()*50);

                // have to have edges both ways
                swap(parent, child);
                g.add_edge(parent, child);
                edge(g, parent, child) = static_cast<edge_weight_type>(rnd.get_random_double()*50);
            }
        }

        pick_random_source_and_sink(rnd, g, source, sink);
    }

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

    template <typename dgraph_type>
    void make_random_chain_graph(
        dlib::rand& rnd,
        dgraph_type& g,
        unsigned long& source,
        unsigned long& sink
    )
    {
        typedef typename dgraph_type::edge_type edge_weight_type;
        g.clear();
        const unsigned int num_nodes = rnd.get_random_32bit_number()%7 + 2;
        g.set_number_of_nodes(num_nodes);

        for (unsigned long i = 1; i < g.number_of_nodes(); ++i)
        {
            g.add_edge(i,i-1);
            g.add_edge(i-1,i);
            edge(g, i, i-1) = static_cast<edge_weight_type>(rnd.get_random_double()*50);
            edge(g, i-1, i) = static_cast<edge_weight_type>(rnd.get_random_double()*50);
        }

        pick_random_source_and_sink(rnd, g, source, sink);
    }

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

    template <typename dgraph_type>
    void make_random_grid_graph(
        dlib::rand& rnd,
        dgraph_type& g,
        unsigned long& source,
        unsigned long& sink
    )
    /*!
        ensures
            - makes a grid graph like the kind used for potts models.
    !*/
    {
        typedef typename dgraph_type::edge_type edge_weight_type;
        g.clear();
        const long nr = rnd.get_random_32bit_number()%2 + 2;
        const long nc = rnd.get_random_32bit_number()%2 + 2;
        g.set_number_of_nodes(nr*nc+2);

        const rectangle rect(0,0,nc-1,nr-1);
        for (long r = 0; r < nr; ++r)
        {
            for (long c = 0; c < nc; ++c)
            {
                const point p(c,r);
                const unsigned long i = p.y()*nc + p.x();

                const point n2(c-1,r);
                if (rect.contains(n2))
                {
                    const unsigned long j = n2.y()*nc + n2.x();
                    g.add_edge(i,j);
                    g.add_edge(j,i);
                    edge(g,i,j) = static_cast<edge_weight_type>(rnd.get_random_double()*50);
                    edge(g,j,i) = static_cast<edge_weight_type>(rnd.get_random_double()*50);
                }

                const point n4(c,r-1);
                if (rect.contains(n4))
                {
                    const unsigned long j = n4.y()*nc + n4.x();
                    g.add_edge(i,j);
                    g.add_edge(j,i);
                    edge(g,i,j) = static_cast<edge_weight_type>(rnd.get_random_double()*50);
                    edge(g,j,i) = static_cast<edge_weight_type>(rnd.get_random_double()*50);
                }
            }
        }

        // use the last two nodes as source and sink.  Also connect them to all the other nodes.
        source = g.number_of_nodes()-1;
        sink = g.number_of_nodes()-2;
        for (unsigned long i = 0; i < g.number_of_nodes()-2; ++i)
        {
            g.add_edge(i,source);
            g.add_edge(source,i);
            g.add_edge(i,sink);
            g.add_edge(sink,i);

            edge(g,i,source) = static_cast<edge_weight_type>(rnd.get_random_double()*50);
            edge(g,source,i) = static_cast<edge_weight_type>(rnd.get_random_double()*50);
            edge(g,i,sink) = static_cast<edge_weight_type>(rnd.get_random_double()*50);
            edge(g,sink,i) = static_cast<edge_weight_type>(rnd.get_random_double()*50);
        }
    }

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

    template <typename min_cut, typename dgraph_type>
    void run_test_on_graphs (
        const min_cut& mc,
        dgraph_type& g1,
        dgraph_type& g2,
        unsigned long source,
        unsigned long sink
    )
    {
        typedef typename dgraph_type::edge_type edge_weight_type;
        using namespace std;

        dlog << LTRACE << "number of nodes: "<< g1.number_of_nodes();
        dlog << LTRACE << "is graph connected: "<< graph_is_connected(g1);
        dlog << LTRACE << "has self loops:     "<< graph_contains_length_one_cycle(g1);
        dlog << LTRACE << "SOURCE_CUT: " << source;
        dlog << LTRACE << "SINK_CUT:   " << sink;
        mc(g1, source, sink);
        brute_force_min_cut(g2, source, sink);

        print_graph(g1);

        // make sure the flow residuals are 0 at the cut locations
        for (unsigned long i = 0; i < g1.number_of_nodes(); ++i)
        {
            for (unsigned long j = 0; j < g1.node(i).number_of_children(); ++j)
            {
                if ((g1.node(i).data == SOURCE_CUT && g1.node(i).child(j).data != SOURCE_CUT) ||
                    (g1.node(i).data != SINK_CUT && g1.node(i).child(j).data == SINK_CUT)
                    )
                {
                    DLIB_TEST_MSG(g1.node(i).child_edge(j) == 0, g1.node(i).child_edge(j));
                }
            }
        }

        // copy the edge weights from g2 back to g1 so we can compute cut scores
        copy_edge_weights(g1, g2);

        DLIB_TEST(g1.number_of_nodes() == g2.number_of_nodes());
        for (unsigned long i = 0; i < g1.number_of_nodes(); ++i)
        {
            dlog << LTRACE << "node " << i << ": " << (int)g1.node(i).data << ", " << (int)g2.node(i).data;
            if (g1.node(i).data != g2.node(i).data)
            {
                edge_weight_type cut_score = graph_cut_score(g1);
                edge_weight_type brute_force_score = graph_cut_score(g2);
                dlog << LTRACE << "graph cut score: "<< cut_score;
                dlog << LTRACE << "brute force score: "<< brute_force_score;

                if (brute_force_score != cut_score)
                    print_graph(g1);
                DLIB_TEST_MSG(brute_force_score == cut_score,std::abs(brute_force_score-cut_score));
            }
        }

    }

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

    template <typename min_cut, typename edge_weight_type>
    void test_graph_cuts(dlib::rand& rnd)
    {
        typedef typename dlib::directed_graph<node_label, edge_weight_type>::kernel_1a_c dgraph_type;
        // we will create two identical graphs.
        dgraph_type g1, g2;
        min_cut mc;

        unsigned long source, sink;

        dlib::rand rnd_copy(rnd);
        make_random_graph(rnd,g1, source, sink);
        make_random_graph(rnd_copy,g2, source, sink);
        run_test_on_graphs(mc, g1, g2, source, sink);

        rnd_copy = rnd;
        make_random_grid_graph(rnd,g1, source, sink);
        make_random_grid_graph(rnd_copy,g2, source, sink);
        run_test_on_graphs(mc, g1, g2, source, sink);

        rnd_copy = rnd;
        make_random_chain_graph(rnd,g1, source, sink);
        make_random_chain_graph(rnd_copy,g2, source, sink);
        run_test_on_graphs(mc, g1, g2, source, sink);

    }

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

    class test_potts_grid_problem
    {
    public:
        test_potts_grid_problem(int seed_) :seed(seed_){}
        int seed;

        long nr() const { return 3;}
        long nc() const { return 3;}

        typedef double value_type;

        value_type factor_value(unsigned long idx) const
        {
            // Copy idx into a char buffer to avoid warnings about violation of strict aliasing
            // rules when murmur_hash3() gets inlined into this function.
            char buf[sizeof(idx)];
            memcpy(buf,&idx,sizeof(idx));
            // now hash the buffer rather than idx.
            return ((double)murmur_hash3(buf, sizeof(buf), seed) - std::numeric_limits<uint32>::max()/2.0)/1000.0;
        }

        value_type factor_value_disagreement(unsigned long idx1, unsigned long idx2) const
        {
            return std::abs(factor_value(idx1+idx2)/10.0);
        }
    };

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

    template <typename prob_type>
    void brute_force_potts_grid_problem(
        const prob_type& prob,
        array2d<unsigned char>& labels
    )
    {
        const unsigned long num = (unsigned long)std::pow(2.0, (double)prob.nr()*prob.nc());

        array2d<unsigned char> temp(prob.nr(), prob.nc());
        unsigned char* data = &temp[0][0];

        double best_score = -std::numeric_limits<double>::infinity();
        for (unsigned long i = 0; i < num; ++i)
        {
            for (unsigned long j = 0; j < temp.size(); ++j)
            {
                unsigned long T = (1)<<j;
                T = (T&i);
                if (T != 0)
                    *(data + j) = SINK_CUT;
                else
                    *(data + j) = SOURCE_CUT;
            }

            double score = potts_model_score(prob, temp);
            if (score > best_score)
            {
                best_score = score;
                assign_image(labels, temp);
            }
        }
    }

    void test_inf()
    {
        graph<double,double>::kernel_1a_c g;
        g.set_number_of_nodes(4);
        g.add_edge(0,1);
        g.add_edge(1,2);
        g.add_edge(2,3);
        g.add_edge(3,0);

        g.node(0).data = std::numeric_limits<double>::infinity();
        g.node(1).data = -std::numeric_limits<double>::infinity();
        g.node(2).data = std::numeric_limits<double>::infinity();
        g.node(3).data = -std::numeric_limits<double>::infinity();

        edge(g,0,1) = 1;
        edge(g,1,2) = 1;
        edge(g,2,3) = 1;
        edge(g,3,0) = 1;

        std::vector<node_label> labels;
        find_max_factor_graph_potts(g, labels);

        DLIB_TEST(labels[0] != 0);
        DLIB_TEST(labels[1] == 0);
        DLIB_TEST(labels[2] != 0);
        DLIB_TEST(labels[3] == 0);

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

        g.node(0).data = std::numeric_limits<double>::infinity();
        g.node(1).data = 0;
        g.node(2).data = 0;
        g.node(3).data = -3;

        edge(g,0,1) = 1;
        edge(g,1,2) = 1;
        edge(g,2,3) = 1;
        edge(g,3,0) = 1;

        find_max_factor_graph_potts(g, labels);

        DLIB_TEST(labels[0] != 0);
        DLIB_TEST(labels[1] != 0);
        DLIB_TEST(labels[2] != 0);
        DLIB_TEST(labels[3] == 0);

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

        g.node(0).data = std::numeric_limits<double>::infinity();
        g.node(1).data = 0;
        g.node(2).data = 0;
        g.node(3).data = -0.1;

        edge(g,0,1) = 1;
        edge(g,1,2) = 1;
        edge(g,2,3) = 1;
        edge(g,3,0) = 1;

        find_max_factor_graph_potts(g, labels);

        DLIB_TEST(labels[0] != 0);
        DLIB_TEST(labels[1] != 0);
        DLIB_TEST(labels[2] != 0);
        DLIB_TEST(labels[3] != 0);

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

        g.node(0).data = std::numeric_limits<double>::infinity();
        g.node(1).data = 0;
        g.node(2).data = 0;
        g.node(3).data = -0.1;

        edge(g,0,1) = 1;
        edge(g,1,2) = 1;
        edge(g,2,3) = 0;
        edge(g,3,0) = 0;

        find_max_factor_graph_potts(g, labels);

        DLIB_TEST(labels[0] != 0);
        DLIB_TEST(labels[1] != 0);
        DLIB_TEST(labels[2] != 0);
        DLIB_TEST(labels[3] == 0);

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

        g.node(0).data = -std::numeric_limits<double>::infinity();
        g.node(1).data = 0;
        g.node(2).data = 0;
        g.node(3).data = 0.1;

        edge(g,0,1) = 1;
        edge(g,1,2) = 1;
        edge(g,2,3) = 0;
        edge(g,3,0) = 0;

        find_max_factor_graph_potts(g, labels);

        DLIB_TEST(labels[0] == 0);
        DLIB_TEST(labels[1] == 0);
        DLIB_TEST(labels[2] == 0);
        DLIB_TEST(labels[3] != 0);

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

        g.node(0).data = -std::numeric_limits<double>::infinity();
        g.node(1).data = std::numeric_limits<double>::infinity();
        g.node(2).data = 0;
        g.node(3).data = 0.1;

        edge(g,0,1) = 1;
        edge(g,1,2) = 1;
        edge(g,2,3) = 0;
        edge(g,3,0) = 0;

        find_max_factor_graph_potts(g, labels);

        DLIB_TEST(labels[0] == 0);
        DLIB_TEST(labels[1] != 0);
        DLIB_TEST(labels[2] != 0);
        DLIB_TEST(labels[3] != 0);

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

        g.node(0).data = -10;
        g.node(1).data = std::numeric_limits<double>::infinity();
        g.node(2).data = 0;
        g.node(3).data = 0.1;

        edge(g,0,1) = std::numeric_limits<double>::infinity();
        edge(g,1,2) = std::numeric_limits<double>::infinity();
        edge(g,2,3) = std::numeric_limits<double>::infinity();
        edge(g,3,0) = std::numeric_limits<double>::infinity();

        find_max_factor_graph_potts(g, labels);

        DLIB_TEST(labels[0] != 0);
        DLIB_TEST(labels[1] != 0);
        DLIB_TEST(labels[2] != 0);
        DLIB_TEST(labels[3] != 0);

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

        g.node(0).data = 10;
        g.node(1).data = -std::numeric_limits<double>::infinity();
        g.node(2).data = 20.05;
        g.node(3).data = -0.1;

        edge(g,0,1) = std::numeric_limits<double>::infinity();
        edge(g,1,2) = 10;
        edge(g,2,3) = std::numeric_limits<double>::infinity();
        edge(g,3,0) = 10;

        find_max_factor_graph_potts(g, labels);

        DLIB_TEST(labels[0] == 0);
        DLIB_TEST(labels[1] == 0);
        DLIB_TEST(labels[2] == 0);
        DLIB_TEST(labels[3] == 0);

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

        g.node(0).data = 10;
        g.node(1).data = -std::numeric_limits<double>::infinity();
        g.node(2).data = 20.2;
        g.node(3).data = -0.1;

        edge(g,0,1) = std::numeric_limits<double>::infinity();
        edge(g,1,2) = 10;
        edge(g,2,3) = std::numeric_limits<double>::infinity();
        edge(g,3,0) = 10;

        find_max_factor_graph_potts(g, labels);

        DLIB_TEST(labels[0] == 0);
        DLIB_TEST(labels[1] == 0);
        DLIB_TEST(labels[2] != 0);
        DLIB_TEST(labels[3] != 0);
    }

    struct potts_pair_image_model
    {
        typedef double value_type;

        template <typename pixel_type1, typename pixel_type2>
        value_type factor_value (
            const pixel_type1& ,
            const pixel_type2& v2
        ) const
        {
            return v2;
        }

        template <typename pixel_type>
        value_type factor_value_disagreement (
            const pixel_type& v1,
            const pixel_type& v2
        ) const
        {
            if (v1 == v2)
                return 10;
            else
                return 0;
        }
    };

    void test_potts_pair_grid()
    {
        array2d<int> img1(40,40);
        array2d<double> img2(40,40);

        assign_all_pixels(img1, -1);
        assign_all_pixels(img2, -1);

        img1[4][4] = 1000;

        img2[4][3] = 1;
        img2[4][4] = 1;
        img2[4][5] = 1;
        img2[3][3] = 1;
        img2[3][4] = 1;
        img2[3][5] = 1;
        img2[5][3] = 1;
        img2[5][4] = 1;
        img2[5][5] = 1;

        array2d<unsigned char> labels;
        find_max_factor_graph_potts(make_potts_grid_problem(potts_pair_image_model(),img2,img1), labels);

        dlog << LINFO << "num true labels: " << sum(matrix_cast<int>(mat(labels)!=0));
        DLIB_TEST(sum(matrix_cast<int>(mat(labels)!=0)) == 9);
        DLIB_TEST(sum(matrix_cast<int>(mat(labels)==0)) == (int)img1.size()-9);

        DLIB_TEST(labels[4][3]);
        DLIB_TEST(labels[4][4]);
        DLIB_TEST(labels[4][5]);
        DLIB_TEST(labels[3][3]);
        DLIB_TEST(labels[3][4]);
        DLIB_TEST(labels[3][5]);
        DLIB_TEST(labels[5][3]);
        DLIB_TEST(labels[5][4]);
        DLIB_TEST(labels[5][5]);
    }

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

    class graph_cuts_tester : public tester
    {
    public:
        graph_cuts_tester (
        ) :
            tester ("test_graph_cuts",
                    "Runs tests on the graph cuts tools.")
        {}

        dlib::rand rnd;

        void perform_test (
        )
        {
            test_potts_pair_grid();
            test_inf();

            for (int i = 0; i < 500; ++i)
            {
                array2d<unsigned char> labels, brute_labels;
                test_potts_grid_problem prob(i);
                find_max_factor_graph_potts(prob, labels);
                brute_force_potts_grid_problem(prob, brute_labels);

                DLIB_TEST(labels.nr() == brute_labels.nr());
                DLIB_TEST(labels.nc() == brute_labels.nc());
                for (long r = 0; r < labels.nr(); ++r)
                {
                    for (long c = 0; c < labels.nc(); ++c)
                    {
                        bool normal = (labels[r][c] != 0);
                        bool brute = (brute_labels[r][c] != 0);
                        DLIB_TEST(normal == brute);
                    }
                }
            }

            for (int i = 0; i < 1000; ++i)
            {
                print_spinner();
                dlog << LTRACE << "test_grpah_cuts<short> iter: " << i;
                test_graph_cuts<min_cut,short>(rnd);
                print_spinner();
                dlog << LTRACE << "test_grpah_cuts<double> iter: " << i;
                test_graph_cuts<min_cut,double>(rnd);
            }

            for (int k = 0; k < 300; ++k)
            {
                dlog << LTRACE << "image_potts_problem iter " << k;
                print_spinner();
                image_potts_problem p(3,3, rnd);
                impl_test_potts_model(p);
            }
            for (int k = 0; k < 300; ++k)
            {
                dlog << LTRACE << "dense_potts_problem iter " << k;
                print_spinner();
                dense_potts_problem p(6, rnd);
                impl_test_potts_model(p);
            }

            for (int k = 0; k < 300; ++k)
            {
                dlog << LTRACE << "dense_potts_problem iter " << k;
                print_spinner();
                test_graph_potts_model(rnd);
            }
        }
    } a;

}


//
// Created by christoph on 08.09.18.
//

#include <Utils/File/Logfile.hpp>
#include <Utils/Convert.hpp>
#include <Math/Math.hpp>
#include <Graphics/Shader/ShaderManager.hpp>
#include <Graphics/Renderer.hpp>

#include <chrono>
#include <iostream>
#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/split.hpp>
#include <GL/glew.h>

#include "MeshSerializer.hpp"
#include "TrajectoryFile.hpp"
#include "TrajectoryLoader.hpp"

using namespace sgl;

static std::vector<glm::vec2> circlePoints2D;

void getPointsOnCircle(std::vector<glm::vec2> &points, const glm::vec2 &center, float radius, int numSegments)
{
    float theta = 2.0f * 3.1415926f / (float)numSegments;
    float tangetialFactor = tan(theta); // opposite / adjacent
    float radialFactor = cos(theta); // adjacent / hypotenuse
    glm::vec2 position(radius, 0.0f);

    for (int i = 0; i < numSegments; i++) {
        points.push_back(position + center);

        // Add the tangent vector and correct the position using the radial factor.
        glm::vec2 tangent(-position.y, position.x);
        position += tangetialFactor * tangent;
        position *= radialFactor;
    }
}

void initializeCircleData(int numSegments, float radius)
{
    circlePoints2D.clear();
    getPointsOnCircle(circlePoints2D, glm::vec2(0.0f, 0.0f), radius, numSegments);
}

/**
 * Returns a oriented and shifted copy of a 2D circle in 3D space.
 * The number
 * @param vertices The list to append the circle points to.
 * @param normals Normal array of the tube to append normals to.
 * @param center The center of the circle in 3D space.
 * @param normal The normal orthogonal to the circle plane.
 * @param lastTangent The tangent of the last circle.
 */
void insertOrientedCirclePoints(std::vector<glm::vec3> &vertices, std::vector<glm::vec3> &normals,
        const glm::vec3 &center, const glm::vec3 &normal, glm::vec3 &lastTangent)
{
    if (circlePoints2D.size() == 0) {
        std::cerr << "Fatal error: circlePoints2D.size() == 0" << std::endl;
        exit(1);
    }

    glm::vec3 tangent, binormal;
    glm::vec3 helperAxis = lastTangent;
    //if (std::abs(glm::dot(helperAxis, normal)) > 0.9f) {
    if (glm::length(glm::cross(helperAxis, normal)) < 0.01f) {
        // If normal == helperAxis
        helperAxis = glm::vec3(0.0f, 1.0f, 0.0f);
    }
    tangent = glm::normalize(helperAxis - normal * glm::dot(helperAxis, normal)); // Gram-Schmidt
    //glm::vec3 tangent = glm::normalize(glm::cross(normal, helperAxis));
    binormal = glm::normalize(glm::cross(normal, tangent));
    lastTangent = tangent;

    // In column-major order
    glm::mat4 tangentFrameMatrix(
            tangent.x,  tangent.y,  tangent.z,  0.0f,
            binormal.x, binormal.y, binormal.z, 0.0f,
            normal.x,   normal.y,   normal.z,   0.0f,
            0.0f,       0.0f,       0.0f,       1.0f);
    glm::mat4 translation(
            1.0f,     0.0f,   .0f,     0.0f,
            0.0f,     1.0f,     0.0f,     0.0f,
            0.0f,     0.0f,     1.0f,     0.0f,
            center.x, center.y, center.z, 1.0f);
    glm::mat4 transform = translation * tangentFrameMatrix;

    for (const glm::vec2 &circlePoint : circlePoints2D) {
        glm::vec4 transformedPoint = transform * glm::vec4(circlePoint.x, circlePoint.y, 0.0f, 1.0f);
        vertices.push_back(glm::vec3(transformedPoint.x, transformedPoint.y, transformedPoint.z));
        glm::vec3 normal = glm::vec3(transformedPoint.x, transformedPoint.y, transformedPoint.z) - center;
        normal = glm::normalize(normal);
        normals.push_back(normal);
    }
}

struct TubeNode
{
    /// Center vertex position
    glm::vec3 center;

    /// Tangent pointing in direction of next node (or negative direction of last node for the final node in the list).
    glm::vec3 tangent;

    /// Circle points (circle with center of tube node, in plane with normal vector of tube node)
    std::vector<glm::vec3> circleVertices;

    std::vector<uint32_t> circleIndices;
};

/**
 * @param pathLineCenters: The (input) path line points to create a tube from.
 * @param pathLineAttributes: The (input) path line point vertex attributes (belonging to pathLineCenters).
 * @param vertices: The (output) vertex points, which are a set of oriented circles around the centers (see above).
 * @param indices: The (output) indices specifying how tube triangles are built from the circle vertices.
 */
template<typename T>
void createTubeRenderData(const std::vector<glm::vec3> &pathLineCenters,
                          const std::vector<T> &pathLineAttributes,
                          std::vector<glm::vec3> &vertices,
                          std::vector<glm::vec3> &normals,
                          std::vector<T> &vertexAttributes,
                          std::vector<uint32_t> &indices)
{
    int n = (int)pathLineCenters.size();
    if (n < 2) {
        sgl::Logfile::get()->writeError("Error in createTube: n < 2");
        return;
    }

    /// Circle points (circle with center of tube node, in plane with normal vector of tube node)
    vertices.reserve(n*circlePoints2D.size());
    normals.reserve(n*circlePoints2D.size());
    vertexAttributes.reserve(n*circlePoints2D.size());
    indices.reserve((n-1)*circlePoints2D.size()*6);

    std::vector<TubeNode> tubeNodes;
    tubeNodes.reserve(n);
    int numVertexPts = 0;

    // Turbulence dataset: Remove fixed point whirls
    glm::vec3 diffFirstLast = pathLineCenters.front() - pathLineCenters.back();
    if (glm::length(diffFirstLast) < 0.01f) {
        return;
    }

    // First, create a list of tube nodes
    glm::vec3 lastNormal = glm::vec3(1.0f, 0.0f, 0.0f);
    for (int i = 0; i < n; i++) {
        glm::vec3 center = pathLineCenters.at(i);

        // Remove invalid line points (used in many scientific datasets to indicate invalid lines).
        const float MAX_VAL = 1e10;
        if (std::fabs(center.x) > MAX_VAL || std::fabs(center.y) > MAX_VAL || std::fabs(center.z) > MAX_VAL) {
            continue;
        }

        glm::vec3 tangent;
        if (i == 0) {
            // First node
            tangent = pathLineCenters.at(i+1) - pathLineCenters.at(i);
        } else if (i == n-1) {
            // Last node
            tangent = pathLineCenters.at(i) - pathLineCenters.at(i-1);
        } else {
            // Node with two neighbors - use both normals
            tangent = pathLineCenters.at(i+1) - pathLineCenters.at(i);
            //normal += pathLineCenters.at(i) - pathLineCenters.at(i-1);
        }

        if (glm::length(tangent) < 0.0001f) {
            // In case the two vertices are almost identical, just skip this path line segment
            continue;
        }

        TubeNode node;
        node.center = pathLineCenters.at(i);
        node.tangent = glm::normalize(tangent);
        insertOrientedCirclePoints(vertices, normals, node.center, node.tangent, lastNormal);
        node.circleIndices.reserve(circlePoints2D.size());
        for (int j = 0; j < circlePoints2D.size(); j++) {
            node.circleIndices.push_back(j + numVertexPts*circlePoints2D.size());
            if (pathLineAttributes.size() > 0) {
                vertexAttributes.push_back(pathLineAttributes.at(i));
            }
        }
        tubeNodes.push_back(node);
        numVertexPts++;
    }

    // Create tube triangles/indices for the vertex data
    /*for (int i = 0; i < numVertexPts-1; i++) {
        std::vector<uint32_t> &circleIndicesCurrent = tubeNodes.at(i).circleIndices;
        std::vector<uint32_t> &circleIndicesNext = tubeNodes.at(i+1).circleIndices;
        for (int j = 0; j < circlePoints2D.size(); j++) {
            // Build two CCW triangles (one quad) for each side
            // Triangle 1
            indices.push_back(circleIndicesCurrent.at(j));
            indices.push_back(circleIndicesCurrent.at((j+1)%circlePoints2D.size()));
            indices.push_back(circleIndicesNext.at((j+1)%circlePoints2D.size()));

            // Triangle 2
            indices.push_back(circleIndicesCurrent.at(j));
            indices.push_back(circleIndicesNext.at((j+1)%circlePoints2D.size()));
            indices.push_back(circleIndicesNext.at(j));
        }
    }*/
    for (int i = 0; i < numVertexPts-1; i++) {
        for (int j = 0; j < circlePoints2D.size(); j++) {
            // Build two CCW triangles (one quad) for each side
            // Triangle 1
            indices.push_back(j + i*circlePoints2D.size());
            indices.push_back((j+1)%circlePoints2D.size() + i*circlePoints2D.size());
            indices.push_back((j+1)%circlePoints2D.size() + (i+1)*circlePoints2D.size());

            // Triangle 2
            indices.push_back(j + i*circlePoints2D.size());
            indices.push_back((j+1)%circlePoints2D.size() + (i+1)*circlePoints2D.size());
            indices.push_back(j + (i+1)*circlePoints2D.size());
        }
    }

    // Only one vertex left -> Output nothing (tube consisting only of one point)
    if (numVertexPts <= 1) {
        vertices.clear();
        normals.clear();
        vertexAttributes.clear();
    }
}
void createTubeRenderData(const std::vector<glm::vec3> &pathLineCenters,
                          std::vector<std::vector<float>> &importanceCriteriaLine,
                          std::vector<glm::vec3> &vertices,
                          std::vector<glm::vec3> &normals,
                          std::vector<std::vector<float>> &importanceCriteriaVertex,
                          std::vector<uint32_t> &indices)
{
    int n = (int)pathLineCenters.size();
    int numImportanceCriteria = (int)importanceCriteriaLine.size();
    if (n < 2) {
        sgl::Logfile::get()->writeError("Error in createTube: n < 2");
        return;
    }

    /// Circle points (circle with center of tube node, in plane with normal vector of tube node)
    vertices.reserve(n*circlePoints2D.size());
    normals.reserve(n*circlePoints2D.size());
    importanceCriteriaVertex.resize(numImportanceCriteria);
    for (int i = 0; i < numImportanceCriteria; i++) {
        importanceCriteriaVertex.at(i).reserve(n);
    }
    indices.reserve((n-1)*circlePoints2D.size()*6);

    // List of all line nodes (points with data)
    std::vector<TubeNode> tubeNodes;
    tubeNodes.reserve(n);
    int numVertexPts = 0;

    // First, create a list of tube nodes
    glm::vec3 lastNormal = glm::vec3(1.0f, 0.0f, 0.0f);
    for (int i = 0; i < n; i++) {
        glm::vec3 center = pathLineCenters.at(i);

        // Remove invalid line points (used in many scientific datasets to indicate invalid lines).
        const float MAX_VAL = 1e10;
        if (std::fabs(center.x) > MAX_VAL || std::fabs(center.y) > MAX_VAL || std::fabs(center.z) > MAX_VAL) {
            continue;
        }

        glm::vec3 tangent;
        if (i == 0) {
            // First node
            tangent = pathLineCenters.at(i+1) - pathLineCenters.at(i);
        } else if (i == n-1) {
            // Last node
            tangent = pathLineCenters.at(i) - pathLineCenters.at(i-1);
        } else {
            // Node with two neighbors - use both normals
            tangent = pathLineCenters.at(i+1) - pathLineCenters.at(i);
            //normal += pathLineCenters.at(i) - pathLineCenters.at(i-1);
        }

        float lineSegmentLength = glm::length(tangent);

        if (lineSegmentLength < 0.0001f) {
            //normal = glm::vec3(1.0f, 0.0f, 0.0f);
            // In case the two vertices are almost identical, just skip this path line segment
            continue;
        }
        tangent = glm::normalize(tangent);

        TubeNode node;
        node.center = pathLineCenters.at(i);
        node.tangent = tangent;
        insertOrientedCirclePoints(vertices, normals, node.center, node.tangent, lastNormal);
        node.circleIndices.reserve(circlePoints2D.size());
        for (int j = 0; j < circlePoints2D.size(); j++) {
            node.circleIndices.push_back(j + numVertexPts*circlePoints2D.size());
            for (int k = 0; k < numImportanceCriteria; k++) {
                importanceCriteriaVertex.at(k).push_back(importanceCriteriaLine.at(k).at(i));
            }
        }
        tubeNodes.push_back(node);
        numVertexPts++;
    }

    // Create tube triangles/indices for the vertex data
    /*for (int i = 0; i < numVertexPts-1; i++) {
        std::vector<uint32_t> &circleIndicesCurrent = tubeNodes.at(i).circleIndices;
        std::vector<uint32_t> &circleIndicesNext = tubeNodes.at(i+1).circleIndices;
        for (int j = 0; j < circlePoints2D.size(); j++) {
            // Build two CCW triangles (one quad) for each side
            // Triangle 1
            indices.push_back(circleIndicesCurrent.at(j));
            indices.push_back(circleIndicesCurrent.at((j+1)%circlePoints2D.size()));
            indices.push_back(circleIndicesNext.at((j+1)%circlePoints2D.size()));

            // Triangle 2
            indices.push_back(circleIndicesCurrent.at(j));
            indices.push_back(circleIndicesNext.at((j+1)%circlePoints2D.size()));
            indices.push_back(circleIndicesNext.at(j));
        }
    }*/
    // Create tube triangles/indices for the vertex data
    for (int i = 0; i < numVertexPts-1; i++) {
        for (int j = 0; j < circlePoints2D.size(); j++) {
            // Build two CCW triangles (one quad) for each side
            // Triangle 1
            indices.push_back(j + i*circlePoints2D.size());
            indices.push_back((j+1)%circlePoints2D.size() + i*circlePoints2D.size());
            indices.push_back((j+1)%circlePoints2D.size() + (i+1)*circlePoints2D.size());

            // Triangle 2
            indices.push_back(j + i*circlePoints2D.size());
            indices.push_back((j+1)%circlePoints2D.size() + (i+1)*circlePoints2D.size());
            indices.push_back(j + (i+1)*circlePoints2D.size());
        }
    }

    // Only one vertex left -> Output nothing (tube consisting only of one point)
    if (numVertexPts <= 1) {
        vertices.clear();
        normals.clear();
        importanceCriteriaVertex.clear();
    }
}

template
void createTubeRenderData<uint32_t>(const std::vector<glm::vec3> &pathLineCenters,
                                    const std::vector<uint32_t> &pathLineAttributes,
                                    std::vector<glm::vec3> &vertices,
                                    std::vector<glm::vec3> &normals,
                                    std::vector<uint32_t> &vertexAttributes,
                                    std::vector<uint32_t> &indices);

void convertTrajectoryDataToBinaryTriangleMesh(
        TrajectoryType trajectoryType,
        const std::string &trajectoriesFilename,
        const std::string &binaryFilename,
        float lineRadius)
{
    auto start = std::chrono::system_clock::now();

    if (trajectoryType == TRAJECTORY_TYPE_RINGS) {
        initializeCircleData(3, lineRadius);
    } else if (trajectoryType == TRAJECTORY_TYPE_ANEURYSM) {
        initializeCircleData(3, lineRadius);
    } else {
        initializeCircleData(3, lineRadius);
    }

    BinaryMesh binaryMesh;
    binaryMesh.submeshes.push_back(BinarySubMesh());
    BinarySubMesh &submesh = binaryMesh.submeshes.front();
    submesh.vertexMode = VERTEX_MODE_TRIANGLES;

    std::vector<glm::vec3> globalVertexPositions;
    std::vector<glm::vec3> globalNormals;
    std::vector<std::vector<float>> globalImportanceCriteria;
    std::vector<uint32_t> globalIndices;

    uint32_t numLines = 0;
    uint32_t numLineSegments = 0;

    Trajectories trajectories = loadTrajectoriesFromFile(trajectoriesFilename, trajectoryType);

    for (size_t i = 0; i < trajectories.size(); i++) {
        Trajectory &trajectory = trajectories.at(i);

        numLines++;
        numLineSegments += trajectory.positions.size() - 1;

        // Create tube render data
        std::vector<glm::vec3> localVertices;
        std::vector<std::vector<float>> importanceCriteriaVertex;
        std::vector<glm::vec3> localNormals;
        std::vector<uint32_t> localIndices;
        createTubeRenderData(trajectory.positions, trajectory.attributes, localVertices, localNormals,
                             importanceCriteriaVertex, localIndices);

        // Local -> global
        if (localVertices.size() > 0) {
            for (size_t i = 0; i < localIndices.size(); i++) {
                globalIndices.push_back(localIndices.at(i) + globalVertexPositions.size());
            }
            globalVertexPositions.insert(globalVertexPositions.end(), localVertices.begin(), localVertices.end());
            globalNormals.insert(globalNormals.end(), localNormals.begin(), localNormals.end());
            if (globalImportanceCriteria.empty()) {
                globalImportanceCriteria.insert(globalImportanceCriteria.end(), importanceCriteriaVertex.begin(),
                                                importanceCriteriaVertex.end());
            } else {
                for (size_t i = 0; i < globalImportanceCriteria.size(); i++) {
                    globalImportanceCriteria.at(i).insert(globalImportanceCriteria.at(i).end(),
                                                          importanceCriteriaVertex.at(i).begin(), importanceCriteriaVertex.at(i).end());
                }
            }
        }
    }

    submesh.material.diffuseColor = glm::vec3(165, 220, 84) / 255.0f;
    submesh.material.opacity = 120 / 255.0f;
    submesh.indices = globalIndices;

    const size_t numIndices = globalIndices.size();
    const size_t numVertices = globalVertexPositions.size();
    const size_t numNormals = globalNormals.size();
    // free memory
    globalIndices.clear(); globalIndices.shrink_to_fit();

    BinaryMeshAttribute positionAttribute;
    positionAttribute.name = "vertexPosition";
    positionAttribute.attributeFormat = ATTRIB_FLOAT;
    positionAttribute.numComponents = 3;
    positionAttribute.data.resize(numVertices * sizeof(glm::vec3));
    memcpy(&positionAttribute.data.front(), &globalVertexPositions.front(), numVertices * sizeof(glm::vec3));
    submesh.attributes.push_back(positionAttribute);

    // free memory
    globalVertexPositions.clear(); globalVertexPositions.shrink_to_fit();

    BinaryMeshAttribute lineNormalsAttribute;
    lineNormalsAttribute.name = "vertexNormal";
    lineNormalsAttribute.attributeFormat = ATTRIB_FLOAT;
    lineNormalsAttribute.numComponents = 3;
    lineNormalsAttribute.data.resize(numNormals * sizeof(glm::vec3));
    memcpy(&lineNormalsAttribute.data.front(), &globalNormals.front(), numNormals * sizeof(glm::vec3));
    submesh.attributes.push_back(lineNormalsAttribute);

    // free memory
    globalNormals.clear(); globalNormals.shrink_to_fit();

    std::vector<std::vector<uint16_t>> globalImportanceCriteriaUnorm;
    packUnorm16ArrayOfArrays(globalImportanceCriteria, globalImportanceCriteriaUnorm);

    for (size_t i = 0; i < globalImportanceCriteriaUnorm.size(); i++) {
        std::vector<uint16_t> &currentAttr = globalImportanceCriteriaUnorm.at(i);
        BinaryMeshAttribute vertexAttribute;
        vertexAttribute.name = "vertexAttribute" + sgl::toString(i);
        vertexAttribute.attributeFormat = ATTRIB_UNSIGNED_SHORT;
        vertexAttribute.numComponents = 1;
        vertexAttribute.data.resize(currentAttr.size() * sizeof(uint16_t));
        memcpy(&vertexAttribute.data.front(), &currentAttr.front(), currentAttr.size() * sizeof(uint16_t));
        submesh.attributes.push_back(vertexAttribute);
    }

    // free memory
    globalImportanceCriteriaUnorm.clear(); globalImportanceCriteriaUnorm.shrink_to_fit();

    auto end = std::chrono::system_clock::now();

    Logfile::get()->writeInfo(std::string() + "Summary: "
                              + sgl::toString(numVertices) + " vertices, "
                              + sgl::toString(numIndices / 3) + " faces, "
                              + sgl::toString(numIndices) + " indices.");
    Logfile::get()->writeInfo(std::string() + "Writing binary mesh...");
    writeMesh3D(binaryFilename, binaryMesh);

    // compute size of renderable geometry;
    float byteSize = positionAttribute.data.size() * sizeof(uint8_t) + lineNormalsAttribute.data.size() * sizeof(uint8_t)
                     + submesh.attributes[0].data.size() * sizeof(uint8_t) + submesh.indices.size() * sizeof(uint32_t);

    float MBSize = byteSize / 1024. / 1024.;

    Logfile::get()->writeInfo(std::string() +  "Byte Size Mesh Structure: " + std::to_string(MBSize) + " MB");
    Logfile::get()->writeInfo(std::string() +  "Num Lines: " + std::to_string(numLines / 1000.) + " Tsd.") ;
    Logfile::get()->writeInfo(std::string() +  "Num LineSegments: " + std::to_string(numLineSegments / 1.0E6) + " Mio");

    auto elapsed =
            std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
    Logfile::get()->writeInfo(std::string() + "Computational time to create binmesh: "
                              + std::to_string(elapsed.count()));

}

void computeLineNormal(const glm::vec3 &tangent, glm::vec3 &normal, const glm::vec3 &lastNormal)
{
    glm::vec3 helperAxis = lastNormal;
    if (glm::length(glm::cross(helperAxis, tangent)) < 0.01f) {
        // If tangent == helperAxis
        helperAxis = glm::vec3(0.0f, 1.0f, 0.0f);
    }
    normal = glm::normalize(helperAxis - tangent * glm::dot(helperAxis, tangent)); // Gram-Schmidt
    //glm::vec3 binormal = glm::normalize(glm::cross(tangent, normal));
}

/**
 * @param pathLineCenters: The (input) path line points to create a tube from.
 * @param pathLineAttributes: The (input) path line point vertex attributes (belonging to pathLineCenters).
 * @param vertices: The (output) vertex points, which are a set of oriented circles around the centers (see above).
 * @param indices: The (output) indices specifying how tube triangles are built from the circle vertices.
 */
void createTangentAndNormalData(std::vector<glm::vec3> &pathLineCenters,
                                std::vector<std::vector<float>> &importanceCriteriaIn,
                                std::vector<glm::vec3> &vertices,
                                std::vector<std::vector<float>> &importanceCriteriaOut,
                                std::vector<glm::vec3> &tangents,
                                std::vector<glm::vec3> &normals,
                                std::vector<uint32_t> &indices)
{
    int n = (int)pathLineCenters.size();
    int numImportanceCriteria = (int)importanceCriteriaIn.size();
    if (n < 2) {
        sgl::Logfile::get()->writeError("Error in createTube: n < 2");
        return;
    }

    vertices.reserve(n);
    importanceCriteriaOut.resize(numImportanceCriteria);
    for (int i = 0; i < numImportanceCriteria; i++) {
        importanceCriteriaOut.at(i).reserve(n);
    }
    tangents.reserve(n);
    normals.reserve(n);
    indices.reserve(n);

    // First, create a list of tube nodes
    glm::vec3 lastNormal = glm::vec3(1.0f, 0.0f, 0.0f);
    for (int i = 0; i < n; i++) {
        glm::vec3 center = pathLineCenters.at(i);

        // Remove invalid line points (used in many scientific datasets to indicate invalid lines).
        const float MAX_VAL = 1e10;
        if (std::fabs(center.x) > MAX_VAL || std::fabs(center.y) > MAX_VAL || std::fabs(center.z) > MAX_VAL) {
            continue;
        }

        glm::vec3 tangent;
        if (i == 0) {
            // First node
            tangent = pathLineCenters.at(i+1) - pathLineCenters.at(i);
        } else if (i == n-1) {
            // Last node
            tangent = pathLineCenters.at(i) - pathLineCenters.at(i-1);
        } else {
            // Node with two neighbors - use both normals
            tangent = pathLineCenters.at(i+1) - pathLineCenters.at(i);
            //normal += pathLineCenters.at(i) - pathLineCenters.at(i-1);
        }
        if (glm::length(tangent) < 0.0001f) {
            // In case the two vertices are almost identical, just skip this path line segment
            continue;
        }
        tangent = glm::normalize(tangent);

        glm::vec3 normal;
        computeLineNormal(tangent, normal, lastNormal);
        lastNormal = normal;

        vertices.push_back(pathLineCenters.at(i));
        for (int j = 0; j < numImportanceCriteria; j++) {
            importanceCriteriaOut.at(j).push_back(importanceCriteriaIn.at(j).at(i));
        }
        tangents.push_back(tangent);
        normals.push_back(normal);
    }

    // Create indices
    for (int i = 0; i < (int)vertices.size() - 1; i++) {
        indices.push_back(i);
        indices.push_back(i+1);
    }
}

struct InputLinePoint {
    glm::vec3 linePoint;
    float lineAttribute;
};
struct OutputLinePoint {
    glm::vec3 linePoint;
    float lineAttribute;
    glm::vec3 lineTangent;
    uint32_t valid; // 0 or 1
    glm::vec3 lineNormal;
    float padding2;
};
struct PathLinePoint {
    glm::vec3 linePointPosition;
    float linePointAttribute;
    glm::vec3 lineTangent;
    float padding1;
    glm::vec3 lineNormal;
    float padding2;
};

struct TubeVertex {
    glm::vec3 vertexPosition;
    float vertexAttribute;
    glm::vec3 vertexNormal;
    float padding;
};

void convertTrajectoryDataToBinaryTriangleMeshGPU(
        TrajectoryType trajectoryType,
        const std::string &trajectoriesFilename,
        const std::string &binaryFilename,
        float lineRadius)
{
    auto start = std::chrono::system_clock::now();
    sgl::ShaderManager->invalidateShaderCache();

    unsigned int NUM_CIRCLE_SEGMENTS = 3;
    if (trajectoryType == TRAJECTORY_TYPE_RINGS) {
        sgl::ShaderManager->addPreprocessorDefine("NUM_CIRCLE_SEGMENTS", NUM_CIRCLE_SEGMENTS);
        sgl::ShaderManager->addPreprocessorDefine("CIRCLE_RADIUS", lineRadius);
    } else if (trajectoryType == TRAJECTORY_TYPE_ANEURYSM) {
        sgl::ShaderManager->addPreprocessorDefine("NUM_CIRCLE_SEGMENTS", NUM_CIRCLE_SEGMENTS);
        sgl::ShaderManager->addPreprocessorDefine("CIRCLE_RADIUS", lineRadius);
    } else {
        sgl::ShaderManager->addPreprocessorDefine("NUM_CIRCLE_SEGMENTS", NUM_CIRCLE_SEGMENTS);
        sgl::ShaderManager->addPreprocessorDefine("CIRCLE_RADIUS", lineRadius);
    }

    BinaryMesh binaryMesh;
    binaryMesh.submeshes.push_back(BinarySubMesh());
    BinarySubMesh &submesh = binaryMesh.submeshes.front();
    submesh.vertexMode = VERTEX_MODE_TRIANGLES;

    std::vector<uint32_t> lineOffsetsInput;
    uint64_t numLinesInput = 0;
    uint64_t numLinePointsInput = 0;

    std::vector<uint32_t> lineOffsetsOutput;
    uint64_t numLinesOutput = 0;
    uint64_t numLinePointsOutput = 0;

    std::vector<InputLinePoint> inputLinePoints;
    std::vector<OutputLinePoint> outputLinePoints;
    std::vector<PathLinePoint> pathLinePoints;

    auto startLoad = std::chrono::system_clock::now();

    Trajectories trajectories = loadTrajectoriesFromFile(trajectoriesFilename, trajectoryType);

    lineOffsetsInput.push_back(0);
    for (size_t i = 0; i < trajectories.size(); i++) {
        Trajectory &trajectory = trajectories.at(i);

        InputLinePoint inputLinePoint;
        for (int j = 0; j < trajectory.positions.size(); j++) {
            inputLinePoint.linePoint = trajectory.positions.at(j);
            inputLinePoint.lineAttribute = trajectory.attributes.at(0).at(j);
            inputLinePoints.push_back(inputLinePoint);
        }

        if (trajectory.positions.size() > 0) {
            numLinePointsInput += trajectory.positions.size();
            numLinesInput++;
        } else {
            continue;
        }
        lineOffsetsInput.push_back(numLinePointsInput);
    }

    auto endLoad = std::chrono::system_clock::now();
    auto elapsedLoad = std::chrono::duration_cast<std::chrono::milliseconds>(endLoad - startLoad);
    Logfile::get()->writeInfo(std::string() + "Computational time to load: " + std::to_string(elapsedLoad.count()));

    const unsigned int WORK_GROUP_SIZE_1D = 256;
    sgl::ShaderManager->addPreprocessorDefine("WORK_GROUP_SIZE_1D", WORK_GROUP_SIZE_1D);
    unsigned int numWorkGroupsOld;
    uint32_t numWorkGroups;
    void *bufferMemory;

    // PART 1: Create line normals & mask invalid line points
    auto startNormals = std::chrono::system_clock::now();
    sgl::GeometryBufferPtr lineOffsetBufferInput = sgl::Renderer->createGeometryBuffer(
            (numLinesInput+1) * sizeof(uint32_t), &lineOffsetsInput.front(),
            SHADER_STORAGE_BUFFER, BUFFER_STATIC);
    sgl::GeometryBufferPtr inputLinePointBuffer = sgl::Renderer->createGeometryBuffer(
            inputLinePoints.size() * sizeof(InputLinePoint), &inputLinePoints.front(),
            SHADER_STORAGE_BUFFER, BUFFER_STATIC);
    sgl::GeometryBufferPtr outputLinePointBuffer = sgl::Renderer->createGeometryBuffer(
            inputLinePoints.size() * sizeof(OutputLinePoint),
            SHADER_STORAGE_BUFFER, BUFFER_STATIC);

    sgl::ShaderProgramPtr createLineNormalsShader = sgl::ShaderManager->getShaderProgram({"CreateLineNormals.Compute"});
    sgl::ShaderManager->bindShaderStorageBuffer(2, lineOffsetBufferInput);
    sgl::ShaderManager->bindShaderStorageBuffer(3, inputLinePointBuffer);
    sgl::ShaderManager->bindShaderStorageBuffer(4, outputLinePointBuffer);
    createLineNormalsShader->setUniform("numLines", static_cast<uint32_t>(numLinesInput));
    numWorkGroupsOld = iceil(numLinesInput, WORK_GROUP_SIZE_1D); // last vector: local work group size
    numWorkGroups = (numLinesInput - 1) / WORK_GROUP_SIZE_1D + 1;

    createLineNormalsShader->dispatchCompute(numWorkGroups);
    glMemoryBarrier(GL_SHADER_STORAGE_BARRIER_BIT);

    bufferMemory = outputLinePointBuffer->mapBuffer(BUFFER_MAP_READ_ONLY);
    outputLinePoints.resize(inputLinePoints.size());
    memcpy(&outputLinePoints.front(), bufferMemory, outputLinePoints.size() * sizeof(OutputLinePoint));
    outputLinePointBuffer->unmapBuffer();
    auto endNormals = std::chrono::system_clock::now();
    auto elapsedNormals = std::chrono::duration_cast<std::chrono::milliseconds>(endNormals - startNormals);
    Logfile::get()->writeInfo(std::string() + "Computational time to create normals: "
            + std::to_string(elapsedNormals.count()));

    // PART 1.2: OutputLinePoint -> PathLinePoint (while removing invalid points)
    auto startCompact = std::chrono::system_clock::now();
    pathLinePoints.reserve(outputLinePoints.size());
    lineOffsetsOutput.push_back(0);
    for (size_t lineID = 0; lineID < numLinesInput; lineID++) {
        size_t linePointsOffset = lineOffsetsInput.at(lineID);
        size_t numLinePoints = lineOffsetsInput.at(lineID+1)-linePointsOffset;

        size_t currentLineNumPointsOutput = 0;

        for (size_t linePointID = 0; linePointID < numLinePoints; linePointID++) {
            OutputLinePoint &outputLinePoint = outputLinePoints.at(linePointsOffset+linePointID);
            if (outputLinePoint.valid == 1) {
                PathLinePoint pathLinePoint;
                pathLinePoint.linePointPosition = outputLinePoint.linePoint;
                pathLinePoint.linePointAttribute = outputLinePoint.lineAttribute;
                pathLinePoint.lineTangent = outputLinePoint.lineTangent;
                pathLinePoint.lineNormal = outputLinePoint.lineNormal;
                pathLinePoints.push_back(pathLinePoint);
                currentLineNumPointsOutput++;
                numLinePointsOutput++;
            }
        }

        if (currentLineNumPointsOutput > 0) {
            numLinesOutput++;
            lineOffsetsOutput.push_back(numLinePointsOutput);
        }
    }
    auto endCompact = std::chrono::system_clock::now();
    auto elapsedCompact = std::chrono::duration_cast<std::chrono::milliseconds>(endCompact - startCompact);
    Logfile::get()->writeInfo(std::string() + "Computational time to compact: "
            + std::to_string(elapsedCompact.count()));

    // PART 2: CreateTubePoints.Compute
    auto startTube = std::chrono::system_clock::now();
    std::vector<TubeVertex> tubeVertices;
    tubeVertices.resize(NUM_CIRCLE_SEGMENTS * pathLinePoints.size());

    sgl::GeometryBufferPtr pathLinePointsBuffer = sgl::Renderer->createGeometryBuffer(
            pathLinePoints.size() * sizeof(PathLinePoint), &pathLinePoints.front(),
            SHADER_STORAGE_BUFFER, BUFFER_STATIC);
    sgl::GeometryBufferPtr tubeVertexBuffer = sgl::Renderer->createGeometryBuffer(
            NUM_CIRCLE_SEGMENTS * pathLinePoints.size() * sizeof(TubeVertex),
            SHADER_STORAGE_BUFFER, BUFFER_STATIC);

    int maxNumWorkGroupsSupported = 0;
    glGetIntegeri_v(GL_MAX_COMPUTE_WORK_GROUP_COUNT, 0, &maxNumWorkGroupsSupported);

    sgl::ShaderProgramPtr createTubePointsShader = sgl::ShaderManager->getShaderProgram({"CreateTubePoints.Compute"});
    sgl::ShaderManager->bindShaderStorageBuffer(2, pathLinePointsBuffer);
    sgl::ShaderManager->bindShaderStorageBuffer(3, tubeVertexBuffer);
    createTubePointsShader->setUniform("numLinePoints", static_cast<uint32_t>(numLinePointsOutput));
    numWorkGroups = iceil(pathLinePoints.size(), WORK_GROUP_SIZE_1D);
    if (numWorkGroups > maxNumWorkGroupsSupported) {
        sgl::Logfile::get()->writeInfo("Info: numWorkGroups > MAX_COMPUTE_WORK_GROUP_COUNT. Switching to CPU fallback.");
        convertTrajectoryDataToBinaryTriangleMesh(trajectoryType, trajectoriesFilename, binaryFilename, lineRadius);
        return;
    }
    createTubePointsShader->dispatchCompute(numWorkGroups);
    glMemoryBarrier(GL_SHADER_STORAGE_BARRIER_BIT);

    bufferMemory = tubeVertexBuffer->mapBuffer(BUFFER_MAP_READ_ONLY);
    memcpy(&tubeVertices.front(), bufferMemory, NUM_CIRCLE_SEGMENTS * pathLinePoints.size() * sizeof(TubeVertex));
    tubeVertexBuffer->unmapBuffer();

    std::vector<glm::vec3> globalVertexPositions;
    std::vector<glm::vec3> globalNormals;
    std::vector<std::vector<float>> globalImportanceCriteria;
    globalVertexPositions.reserve(tubeVertices.size());
    globalNormals.reserve(tubeVertices.size());
    globalImportanceCriteria.resize(1);
    globalImportanceCriteria.at(0).reserve(tubeVertices.size());
    for (TubeVertex &tubeVertex : tubeVertices) {
        globalVertexPositions.push_back(tubeVertex.vertexPosition);
        globalNormals.push_back(tubeVertex.vertexNormal);
        globalImportanceCriteria.at(0).push_back(tubeVertex.vertexAttribute);
    }
    auto endTube = std::chrono::system_clock::now();
    auto elapsedTube = std::chrono::duration_cast<std::chrono::milliseconds>(endTube - startTube);
    Logfile::get()->writeInfo(std::string() + "Computational time to create tube vertices: "
                              + std::to_string(elapsedTube.count()));

    // PART 3: CreateTubeIndices.Compute
    auto startIndices = std::chrono::system_clock::now();
    std::vector<uint32_t> tubeIndices;
    size_t numLineSegments = numLinePointsOutput - numLinesOutput;
    size_t numIndices = numLineSegments*NUM_CIRCLE_SEGMENTS*6;
    tubeIndices.resize(numIndices);

    sgl::GeometryBufferPtr lineOffsetBufferOutput = sgl::Renderer->createGeometryBuffer(
            (numLinesOutput+1) * sizeof(uint32_t), &lineOffsetsOutput.front(),
            SHADER_STORAGE_BUFFER, BUFFER_STATIC);
    sgl::GeometryBufferPtr tubeIndexBuffer = sgl::Renderer->createGeometryBuffer(
            numIndices * sizeof(uint32_t),
            SHADER_STORAGE_BUFFER, BUFFER_STATIC);

    sgl::ShaderProgramPtr createTubeIndicesShader = sgl::ShaderManager->getShaderProgram({"CreateTubeIndices.Compute"});
    sgl::ShaderManager->bindShaderStorageBuffer(2, lineOffsetBufferOutput);
    sgl::ShaderManager->bindShaderStorageBuffer(3, tubeIndexBuffer);
    createTubeIndicesShader->setUniform("numLines", static_cast<uint32_t>(numLinesOutput));
    numWorkGroups = iceil(numLinesOutput, WORK_GROUP_SIZE_1D); // last vector: local work group size
    createTubeIndicesShader->dispatchCompute(numWorkGroups);
    glMemoryBarrier(GL_SHADER_IMAGE_ACCESS_BARRIER_BIT);

    bufferMemory = tubeIndexBuffer->mapBuffer(BUFFER_MAP_READ_ONLY);
    memcpy(&tubeIndices.front(), bufferMemory, numIndices * sizeof(uint32_t));
    tubeIndexBuffer->unmapBuffer();
    auto endIndices = std::chrono::system_clock::now();
    auto elapsedIndices = std::chrono::duration_cast<std::chrono::milliseconds>(endIndices - startIndices);
    Logfile::get()->writeInfo(std::string() + "Computational time to create tube indices: "
            + std::to_string(elapsedIndices.count()));

    sgl::ShaderManager->removePreprocessorDefine("WORK_GROUP_SIZE_1D");
    sgl::ShaderManager->removePreprocessorDefine("NUM_CIRCLE_SEGMENTS");
    sgl::ShaderManager->removePreprocessorDefine("CIRCLE_RADIUS");
    sgl::ShaderManager->unbindShader();

    // Normalize data for rings
    auto startPost = std::chrono::system_clock::now();

    submesh.material.diffuseColor = glm::vec3(165, 220, 84) / 255.0f;
    submesh.material.opacity = 120 / 255.0f;
    submesh.indices = tubeIndices;

    const size_t numIndicesTubes = tubeIndices.size();
    const size_t numVertices = globalVertexPositions.size();
    const size_t numNormals = globalNormals.size();
    // free memory
    tubeIndices.clear(); tubeIndices.shrink_to_fit();

    BinaryMeshAttribute positionAttribute;
    positionAttribute.name = "vertexPosition";
    positionAttribute.attributeFormat = ATTRIB_FLOAT;
    positionAttribute.numComponents = 3;
    positionAttribute.data.resize(numVertices * sizeof(glm::vec3));
    memcpy(&positionAttribute.data.front(), &globalVertexPositions.front(), numVertices * sizeof(glm::vec3));
    submesh.attributes.push_back(positionAttribute);

    // free memory
    globalVertexPositions.clear(); globalVertexPositions.shrink_to_fit();

    BinaryMeshAttribute lineNormalsAttribute;
    lineNormalsAttribute.name = "vertexNormal";
    lineNormalsAttribute.attributeFormat = ATTRIB_FLOAT;
    lineNormalsAttribute.numComponents = 3;
    lineNormalsAttribute.data.resize(numNormals * sizeof(glm::vec3));
    memcpy(&lineNormalsAttribute.data.front(), &globalNormals.front(), numNormals * sizeof(glm::vec3));
    submesh.attributes.push_back(lineNormalsAttribute);

    // free memory
    globalNormals.clear(); globalNormals.shrink_to_fit();

    std::vector<std::vector<uint16_t>> globalImportanceCriteriaUnorm;
    packUnorm16ArrayOfArrays(globalImportanceCriteria, globalImportanceCriteriaUnorm);

    for (size_t i = 0; i < globalImportanceCriteriaUnorm.size(); i++) {
        std::vector<uint16_t> &currentAttr = globalImportanceCriteriaUnorm.at(i);
        BinaryMeshAttribute vertexAttribute;
        vertexAttribute.name = "vertexAttribute" + sgl::toString(i);
        vertexAttribute.attributeFormat = ATTRIB_UNSIGNED_SHORT;
        vertexAttribute.numComponents = 1;
        vertexAttribute.data.resize(currentAttr.size() * sizeof(uint16_t));
        memcpy(&vertexAttribute.data.front(), &currentAttr.front(), currentAttr.size() * sizeof(uint16_t));
        submesh.attributes.push_back(vertexAttribute);
    }
    auto endPost = std::chrono::system_clock::now();
    auto elapsedPost = std::chrono::duration_cast<std::chrono::milliseconds>(endPost - startPost);
    Logfile::get()->writeInfo(std::string() + "Computational time post-process: " + std::to_string(elapsedPost.count()));

    auto end = std::chrono::system_clock::now();

    Logfile::get()->writeInfo(std::string() + "Summary: "
                              + sgl::toString(numVertices) + " vertices, "
                              + sgl::toString(numIndicesTubes / 3) + " faces, "
                              + sgl::toString(numIndicesTubes) + " indices.");
    Logfile::get()->writeInfo(std::string() + "Writing binary mesh...");
    writeMesh3D(binaryFilename, binaryMesh);

    // compute size of renderable geometry;
    float byteSize = positionAttribute.data.size() * sizeof(uint8_t) + lineNormalsAttribute.data.size() * sizeof(uint8_t)
                     + submesh.attributes[0].data.size() * sizeof(uint8_t) + submesh.indices.size() * sizeof(uint32_t);

    float MBSize = byteSize / 1024. / 1024.;

    // free memory
    submesh.attributes.clear(); submesh.attributes.shrink_to_fit();

    Logfile::get()->writeInfo(std::string() +  "Byte Size Mesh Structure: " + std::to_string(MBSize) + " MB");
    Logfile::get()->writeInfo(std::string() +  "Num Lines: " + std::to_string(numLinesOutput / 1000.) + " Tsd.") ;
    Logfile::get()->writeInfo(std::string() +  "Num Line Points: " + std::to_string(numLinePointsOutput / 1.0E6) + " Mio");

    auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
    Logfile::get()->writeInfo(std::string() + "Computational time to create binmesh: "
                              + std::to_string(elapsed.count()));
}

void convertTrajectoryDataToBinaryLineMesh(
        TrajectoryType trajectoryType,
        const std::string &trajectoriesFilename,
        const std::string &binaryFilename)
{
    auto start = std::chrono::system_clock::now();

    BinaryMesh binaryMesh;
    binaryMesh.submeshes.push_back(BinarySubMesh());
    BinarySubMesh &submesh = binaryMesh.submeshes.front();
    submesh.vertexMode = VERTEX_MODE_LINES;

    std::vector<glm::vec3> globalVertexPositions;
    std::vector<glm::vec3> globalNormals;
    std::vector<glm::vec3> globalTangents;
    std::vector<std::vector<float>> globalImportanceCriteria;
    std::vector<uint32_t> globalIndices;

    Trajectories trajectories = loadTrajectoriesFromFile(trajectoriesFilename, trajectoryType);

    for (size_t i = 0; i < trajectories.size(); i++) {
        Trajectory &trajectory = trajectories.at(i);

        // Create tube render data
        std::vector<glm::vec3> localVertices;
        std::vector<glm::vec3> localTangents;
        std::vector<glm::vec3> localNormals;
        std::vector<uint32_t> localIndices;
        std::vector<std::vector<float>> importanceCriteriaOut;
        createTangentAndNormalData(trajectory.positions, trajectory.attributes, localVertices,
                                   importanceCriteriaOut, localTangents, localNormals, localIndices);

        // Local -> global
        if (localVertices.size() > 0) {
            for (size_t i = 0; i < localIndices.size(); i++) {
                globalIndices.push_back(localIndices.at(i) + globalVertexPositions.size());
            }
            globalVertexPositions.insert(globalVertexPositions.end(), localVertices.begin(), localVertices.end());
            globalTangents.insert(globalTangents.end(), localTangents.begin(), localTangents.end());
            globalNormals.insert(globalNormals.end(), localNormals.begin(), localNormals.end());
            if (globalImportanceCriteria.empty()) {
                globalImportanceCriteria.insert(globalImportanceCriteria.end(), importanceCriteriaOut.begin(),
                                                importanceCriteriaOut.end());
            } else {
                for (size_t i = 0; i < globalImportanceCriteria.size(); i++) {
                    globalImportanceCriteria.at(i).insert(globalImportanceCriteria.at(i).end(),
                                                          importanceCriteriaOut.at(i).begin(),
                                                          importanceCriteriaOut.at(i).end());
                }
            }
        }
    }

    submesh.material.diffuseColor = glm::vec3(165, 220, 84) / 255.0f;
    submesh.material.opacity = 120 / 255.0f;
    submesh.indices = globalIndices;

    const size_t numIndices = globalIndices.size();
    const size_t numVertices = globalVertexPositions.size();
    const size_t numNormals = globalNormals.size();
    const size_t numTangents = globalTangents.size();
    // free memory
    globalIndices.clear(); globalIndices.shrink_to_fit();

    BinaryMeshAttribute positionAttribute;
    positionAttribute.name = "vertexPosition";
    positionAttribute.attributeFormat = ATTRIB_FLOAT;
    positionAttribute.numComponents = 3;
    positionAttribute.data.resize(numVertices * sizeof(glm::vec3));
    memcpy(&positionAttribute.data.front(), &globalVertexPositions.front(), numVertices * sizeof(glm::vec3));
    submesh.attributes.push_back(positionAttribute);

    // free memory
    globalVertexPositions.clear(); globalVertexPositions.shrink_to_fit();

    BinaryMeshAttribute lineNormalsAttribute;
    lineNormalsAttribute.name = "vertexLineNormal";
    lineNormalsAttribute.attributeFormat = ATTRIB_FLOAT;
    lineNormalsAttribute.numComponents = 3;
    lineNormalsAttribute.data.resize(numNormals * sizeof(glm::vec3));
    memcpy(&lineNormalsAttribute.data.front(), &globalNormals.front(), numNormals * sizeof(glm::vec3));
    submesh.attributes.push_back(lineNormalsAttribute);

    // free memory
    globalNormals.clear(); globalNormals.shrink_to_fit();

    BinaryMeshAttribute lineTangentAttribute;
    lineTangentAttribute.name = "vertexLineTangent";
    lineTangentAttribute.attributeFormat = ATTRIB_FLOAT;
    lineTangentAttribute.numComponents = 3;
    lineTangentAttribute.data.resize(numTangents * sizeof(glm::vec3));
    memcpy(&lineTangentAttribute.data.front(), &globalTangents.front(), numTangents * sizeof(glm::vec3));
    submesh.attributes.push_back(lineTangentAttribute);

    // free memory
    globalTangents.clear(); globalTangents.shrink_to_fit();

    std::vector<std::vector<uint16_t>> globalImportanceCriteriaUnorm;
    packUnorm16ArrayOfArrays(globalImportanceCriteria, globalImportanceCriteriaUnorm);

    for (size_t i = 0; i < globalImportanceCriteriaUnorm.size(); i++) {
        std::vector<uint16_t> &currentAttr = globalImportanceCriteriaUnorm.at(i);
        BinaryMeshAttribute vertexAttribute;
        vertexAttribute.name = "vertexAttribute" + sgl::toString(i);
        vertexAttribute.attributeFormat = ATTRIB_UNSIGNED_SHORT;
        vertexAttribute.numComponents = 1;
        vertexAttribute.data.resize(currentAttr.size() * sizeof(uint16_t));
        memcpy(&vertexAttribute.data.front(), &currentAttr.front(), currentAttr.size() * sizeof(uint16_t));
        submesh.attributes.push_back(vertexAttribute);
    }

    // free memory
    globalImportanceCriteriaUnorm.clear(); globalImportanceCriteriaUnorm.shrink_to_fit();

    auto end = std::chrono::system_clock::now();

    Logfile::get()->writeInfo(std::string() + "Summary: "
                              + sgl::toString(numVertices) + " vertices, "
                              + sgl::toString(numIndices / 3) + " faces, "
                              + sgl::toString(numIndices) + " indices.");
    Logfile::get()->writeInfo(std::string() + "Writing binary mesh...");
    writeMesh3D(binaryFilename, binaryMesh);

    auto elapsed =
            std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
    Logfile::get()->writeInfo(std::string() + "Computational time to create binmesh: "
                        + std::to_string(elapsed.count()));
}


#include "Generic/common/leak_detection.h"

#include <string>
#include <vector>

#include <boost/algorithm/string/predicate.hpp>
#include "Generic/common/Symbol.h"

#include "DistributionalUtil.h"

float DistributionalUtil::calculateAvgScore(const float& s1, const float& s2) {
	float avgS = -1;

	if( s1!=-1 && s2!=-1 )
		avgS = (s1 + s2)/2;

	return avgS;
}

int DistributionalUtil::scoreToBin(const float& score) {
	int bin;

        if(score<0.5) {
        	bin = 5;
        }
        else if( (0.5<=score) && (score<0.6) ) {
                bin = 6;
        }
        else if( (0.6<=score) && (score<0.7) ) {
                bin = 7;
        }
        else if( (0.7<=score) && (score<0.8) ) {
                bin = 8;
        }
        else if( (0.8<=score) && (score<0.9) ) {
                bin = 9;
                }
        else if(0.9<=score) {
                bin = 10;
        }

	return bin;
}

/*
  10/15/2013 : Yee Seng Chan
  The scoreToBins is temporarily there for an alternative way of representing scores, as a set of inequalities,
  instead of as a single bin as currently represented by the scoreToBin function.

std::vector<std::wstring> DistributionalUtil::scoreToBins(const float& score) {
	std::vector<std::wstring> bins;

        if(score<0.5) {
        	bins.push_back(L"<=0");
        }
        else if( (0.5<=score) && (score<0.6) ) {
                bins.push_back(L">0");
                bins.push_back(L"<=5");
        }
        else if( (0.6<=score) && (score<0.7) ) {
                bins.push_back(L">0");
                bins.push_back(L">5");
                bins.push_back(L"<=6");
        }
        else if( (0.7<=score) && (score<0.8) ) {
                bins.push_back(L">0");
                bins.push_back(L">5");
                bins.push_back(L">6");
                bins.push_back(L"<=7");
        }
        else if( (0.8<=score) && (score<0.9) ) {
                bins.push_back(L">0");
                bins.push_back(L">5");
                bins.push_back(L">6");
                bins.push_back(L">7");
                bins.push_back(L"<=8");
        }
        else if(0.9<=score) {
                bins.push_back(L">0");
                bins.push_back(L">5");
                bins.push_back(L">6");
                bins.push_back(L">7");
                bins.push_back(L">8");
        }

	return bins;
}
*/

Symbol DistributionalUtil::determinePosType(const Symbol& posTag) {
	if(boost::algorithm::starts_with(posTag.to_string(), L"NN"))
		return Symbol(L"N");
	else if(boost::algorithm::starts_with(posTag.to_string(), L"JJ"))
		return Symbol(L"J");
	else if(boost::algorithm::starts_with(posTag.to_string(), L"VB"))
		return Symbol(L"V");
	else
		return Symbol();

	return Symbol();
}


/*
 * Copyright 2015 Christoph Jud (christoph.jud@unibas.ch)
 *
 * 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 <iostream>
#include <memory>
#include <ctime>
#include <cmath>

#include <boost/random.hpp>

#include "GaussianProcess.h"
#include "Kernel.h"
#include "KernelUtils.h"
#include "MatrixIO.h"

using namespace gpr;

typedef Kernel<double>                      KernelType;
typedef std::shared_ptr<KernelType>         KernelTypePointer;
typedef WhiteKernel<double>                 WhiteKernelType;
typedef std::shared_ptr<WhiteKernelType>    WhiteKernelTypePointer;
typedef GaussianProcess<double> GaussianProcessType;
typedef std::shared_ptr<GaussianProcessType> GaussianProcessTypePointer;
typedef GaussianProcessType::VectorType VectorType;
typedef GaussianProcessType::MatrixType MatrixType;

void Test1(){
    /*
     * Test 1: construct, regress, io of highly general kernel
     */
    std::cout << "Test 1.1: construct, regress, of highly general kernel..." << std::flush;

    // ground truth periodic variable
    auto f = [](double x)->double { return std::sin(x)*std::cos(2.2*std::sin(x)); };

    double interval_start = 0;
    double interval_end = 5 * 2*M_PI; // full interval
    double interval_step = 0.1;

    //--------------------------------------------------------------------------------
    // generating ground truth
    unsigned gt_size = (interval_end-interval_start) / interval_step;
    VectorType y(gt_size);
    for(unsigned i=0; i<gt_size; i++){
        y[i] = f(interval_start + i*interval_step);
    }

    //--------------------------------------------------------------------------------
    // perform training
    double noise = 0.01;
    static boost::minstd_rand randgen(static_cast<unsigned>(time(0)));
    static boost::normal_distribution<> dist(0, noise);
    static boost::variate_generator<boost::minstd_rand, boost::normal_distribution<> > r(randgen, dist);

    double interval_training_end = 2 * 2*M_PI; // interval to train
    unsigned number_of_samples = 50;

    std::vector<double> params;
    params.push_back(1);
    params.push_back(2);
    params.push_back(3);
    params.push_back(4);
    params.push_back(5);
    params.push_back(6);
    params.push_back(7);
    params.push_back(8);
    params.push_back(9);
    params.push_back(101);
    params.push_back(202);
    params.push_back(303);
    params.push_back(404);

    KernelTypePointer k = GetGeneralKernel(params);
    std::string k_string = k->ToString();

    GaussianProcessTypePointer gp(new GaussianProcessType(k));
    gp->SetSigma(0); // noise

    // add samples
    double training_step_size = (interval_training_end - interval_start) / number_of_samples;
    for(unsigned i=0; i<number_of_samples; i++){
        VectorType x(1);
        x(0) = interval_start + i*training_step_size;

        VectorType y(1);
        y(0) = f(x(0)) + r();

        gp->AddSample(x, y);
    }
    gp->Initialize();

    //--------------------------------------------------------------------------------
    // predict full intervall
    VectorType y_predict(gt_size);
    for(unsigned i=0; i<gt_size; i++){
        VectorType x(1);
        x(0) = interval_start + i*interval_step;
        y_predict[i] = gp->Predict(x)(0);
    }

    double err = (y-y_predict).norm(); // here we are not interested in an accurate regression
    if(err>6){
        std::stringstream ss; ss<<err; throw ss.str();
    }
    else{
        std::cout << "\t[passed]." << std::endl;
    }

    std::cout << "Test 1.2: save/load of highly general kernel..." << std::flush;

    gp->Save("/tmp/gp_io_test-");

    WhiteKernelTypePointer k_dummy(new WhiteKernelType(1));
    GaussianProcessTypePointer gp_read(new GaussianProcessType(k_dummy));

    try{
        gp_read->Load("/tmp/gp_io_test-");
    }
    catch(std::string& s){
        std::cout << s << std::endl;
    }

    std::string k_string_read = gp_read->GetKernel()->ToString();

    if(*gp.get() == *gp_read.get() && k_string_read.compare(k_string)==0){
        std::cout << "\t\t\t[passed]." << std::endl;
    }
    else{
        throw std::string("comparison");
    }
}

int main (int argc, char *argv[]){
    std::cout << "Highly general kernel test: " << std::endl;
    try{
        Test1();
    }
    catch(std::string& s){
        std::cout << "[failed] Error: " << s << std::endl;
        return -1;
    }

    return 0;
}


#include "lola/Robot.h"
#include "lepp3/models/Coordinate.h"
#include "lepp3/models/SurfaceModel.h"

#include <Eigen/Geometry>
#include <limits>

using namespace lepp;

bool Robot::isInRobotBoundary(ObjectModel const& model) const {
  int obj_id = model.id();
  Coordinate model_center = model.center_point();
  Coordinate robot_position = this->robot_position();
  // Now find the distance between the two coordinates, giving the
  // (rough) distance between the robot and the object.
  double const squared_dist = (model_center - robot_position).square_norm();

  // The object is considered to be in the robot's boundary if closer
  // than a particular threshold.
  return squared_dist < inner_zone_square_radius_;
}

// checks to see if the robot is near or on a given surface
// If the robot's center is within the surface's convex hull or near
// to one of the edges which forms the convex hull, this returns TRUE.
bool Robot::isInRobotBoundary(SurfaceModel const& model) const {
  int obj_id = model.id();

  Coordinate robot_position = this->robot_position();
  PointT robot_center;
  robot_center.x = robot_position.x;
  robot_center.y = robot_position.y;
  robot_center.z = robot_position.z;

  // check if robot is inside (or standing on) the surface
  bool isInside = pcl::isPointIn2DPolygon(robot_center, *(model.get_hull()));
  if (isInside) {
    return true;
  }

  // check if robot is close to the edge of the surface
  double sq_min_dist_to_poly = std::numeric_limits<double>::max();

  const size_t hull_point_count = model.get_hull()->points.size();
  for (size_t i = 1; i <= hull_point_count; ++i) {
    PointT const& point1 = model.get_hull()->points[i % hull_point_count];
    PointT const& point2 = model.get_hull()->points[i - 1];

    Eigen::Vector3d p1_to_p2 = {(point2 - point1).x, (point2 - point1).y, (point2 - point1).z};
    Eigen::Vector3d p1_to_robot = {(robot_center - point1).x, (robot_center - point1).y, (robot_center - point1).z}; // I hate myself
    Eigen::Vector3d p2_to_robot = {(robot_center - point2).x, (robot_center - point2).y, (robot_center - point2).z}; // I hate myself

    auto r = p1_to_robot.dot(p1_to_p2 / p1_to_p2.norm()); //scalar projection of p1_to_robot into p1_to_p2
    r /= p1_to_p2.norm(); //this checks if the projection of p1_to_robot into p1_to_p2 lies inside or outside the segment

    double sq_dist_to_poly = 0; // distance to this edge
    if (r < 0) { // robot lies outside of the segment, closer to p1
      sq_dist_to_poly = p1_to_robot.squaredNorm();
    } else if (r > 1) { // robot lies outside of the segment, closer to p2
      sq_dist_to_poly = p2_to_robot.squaredNorm();
    } else { // robot lies inside the segment
      sq_dist_to_poly = p1_to_robot.squaredNorm() - r * p1_to_p2.squaredNorm();
    }

    sq_min_dist_to_poly = std::min(sq_dist_to_poly, sq_min_dist_to_poly);
  }

  // The surface is considered to be in the robot's boundary if closer
  // than a particular threshold or the robot's position is within the
  // bounds of the surface's convex hull.
  return sq_min_dist_to_poly < inner_zone_square_radius_;
}

lepp::Coordinate Robot::robot_position() const {
  lepp::LolaKinematicsParams params = pose_service_.getParams();
  return lepp::PoseService::getRobotPosition(params);
}


#include <boost/math/distributions/extreme_value.hpp>


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

  @copyright 2016 NumScale SAS

  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_ARCH_COMMON_SCALAR_FUNCTION_TANH_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_TANH_HPP_INCLUDED

#include <boost/simd/arch/common/detail/generic/tanh_kernel.hpp>
#include <boost/simd/constant/mtwo.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( tanh_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<bd::floating_<A0> >
                          )
  {

    BOOST_FORCEINLINE A0 operator() ( A0  a0) const BOOST_NOEXCEPT
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) is less than 5/8 sinh is computed using a polynomial(float)
      // (respectively rational(double)) approx from cephes.
      // else
      // tanh(a0) is  sign(a0)*(2/(exp(2*x)+1)+1)
      //////////////////////////////////////////////////////////////////////////////
      A0 x = bs::abs(a0);
      if( x < Ratio<A0, 5, 8>())
      {
       return detail::tanh_kernel<A0>::tanh(a0, sqr(x));
      }
      else
      {
       A0 r = fma(Mtwo<A0>(), rec(inc(exp(x+x))), One<A0>());
       return bitwise_xor(r, bitofsign(a0));
      }
    }
  };
} } }

#endif