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

#include "graph.hh"

#include "graph_filtering.hh"

#include "graph_selectors.hh"
#include "graph_properties.hh"

#include <boost/mpl/quote.hpp>
#include <boost/graph/breadth_first_search.hpp>

#include <cmath>

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

typedef pair<double, double> point_t;

point_t interpolate(const point_t& p1, const point_t& p2, double r = 0.5)
{
    point_t ret;
    ret.first = (1 - r) * p1.first + p2.first * r;
    ret.second = (1 - r) * p1.second + p2.second * r;
    return ret;
}

double dist(point_t& p1, point_t& p2)
{
    return sqrt(pow(p1.first - p2.first, 2) + pow(p1.second - p2.second, 2));
}

void to_bezier(const vector<point_t> &x, vector<point_t>& ncp)
{
    vector<point_t> cp(x.size() + 6);
    for (size_t i = 0; i < 3; ++i)
        cp[i] = x[0];
    for (size_t i = 0; i < x.size(); ++i)
        cp[i + 3] = x[i];
    for (size_t i = cp.size() - 3; i < cp.size(); ++i)
        cp[i] = x.back();

    vector<point_t> one_thirds(cp.size() - 1);
    vector<point_t> two_thirds(cp.size() - 1);

    for (size_t i = 0; i < cp.size() - 1; ++i)
    {
        const point_t& p1 = cp[i];
        const point_t& p2 = cp[i + 1];
        one_thirds[i] = interpolate(p1, p2, 1./3);
        two_thirds[i] = interpolate(p2, p1, 1./3);
    }

    ncp.resize((cp.size() - 3) * 3);
    for (size_t i = 0; i < cp.size() - 3; ++i)
    {
        size_t pos = i * 3;
        ncp[pos] = one_thirds[i + 1];
        ncp[pos + 1] = two_thirds[i + 1];
        ncp[pos + 2] = interpolate(two_thirds[i + 1], one_thirds[i + 2]);
    }
}

void transform(vector<point_t>& cp)
{
    point_t origin = cp[0];
    for (auto& xy : cp)
    {
        xy.first -= origin.first;
        xy.second -= origin.second;
    }

    double t = atan2(cp.back().second - cp.front().second,
                     cp.back().first - cp.front().first);

    for (auto& xy : cp)
    {
        double x = xy.first;
        double y = xy.second;

        xy.first = cos(t) * x + sin(t) * y;
        xy.second = -sin(t) * x + cos(t) * y;
    }

    point_t d;
    d.first = cp.back().first - cp.front().first;
    d.second = cp.back().second - cp.front().second;
    double r = sqrt(d.first * d.first + d.second * d.second);

    for (auto& xy : cp)
        xy.first /= r;

    d.first = d.second = 0;
    cp.insert(cp.begin(), d);
}

template <class PosProp>
void get_control_points(vector<size_t>& path, PosProp& pos, double beta,
                        vector<point_t>& ncp)
{
    size_t L = path.size();
    vector<point_t> cp(L);
    for (size_t i = 0; i < L; ++i)
    {
        auto& p = pos[path[i]];
        if (p.size() < 2)
            p.resize(2);
        cp[i].first = p[0];
        cp[i].second = p[1];
    }

    ncp.resize(L);
    for (size_t i = 0; i < L; ++i)
    {
        ncp[i].first = beta * cp[i].first + (1 - beta) * (cp[0].first + (cp.back().first - cp[0].first) * i / (L - 1.));
        ncp[i].second = beta * cp[i].second + (1 - beta) * (cp[0].second + (cp.back().second - cp[0].second) * i / (L - 1.));
    }
}

template <class Graph>
void tree_path(Graph& g, size_t s, size_t t, vector<size_t>& path,
               size_t max_depth)
{
    vector<size_t> s_root;
    vector<size_t> t_root;
    s_root.push_back(s);
    t_root.push_back(t);

    size_t v = s;
    size_t u = t;

    while (v != u && s_root.size() < max_depth)
    {
        typename graph_traits<Graph>::in_edge_iterator e, e_end;
        tie(e, e_end) = in_edges(v, g);
        if (e == e_end)
            throw GraphException("Invalid hierarchical tree: No path from source to target.");
        v = source(*e, g);
        s_root.push_back(v);

        tie(e, e_end) = in_edges(u, g);
        if (e == e_end)
            throw GraphException("Invalid hierarchical tree: No path from source to target.");
        u = source(*e, g);
        if (u != v)
            t_root.push_back(u);
    }
    path = s_root;
    std::copy(t_root.rbegin(), t_root.rend(), std::back_inserter(path));
}

template <class Graph>
void graph_path(Graph& g, size_t s, size_t t, vector<size_t>& path)
{
    typename property_map_type::apply<size_t,
                                      typename property_map<Graph, vertex_index_t>::type>::type
        cpred;
    auto pred = cpred.get_unchecked(num_vertices(g));

    undirected_adaptor<Graph> ug(g);

    boost::breadth_first_search(ug, s,
                                boost::visitor(
                                    boost::make_bfs_visitor(
                                        boost::record_predecessors(
                                            pred,
                                            boost::on_tree_edge()))));
    size_t pos = t;
    path.push_back(pos);
    while (pos != s)
    {
        pos = pred[pos];
        path.push_back(pos);
    }
    std::reverse(path.begin(), path.end());
}

template<class T>
void pack(vector<point_t>& cp, vector<T>& ncp)
{
    ncp.resize(cp.size() * 2);
    for (size_t i = 0; i < cp.size(); ++i)
    {
        ncp[2 * i] = cp[i].first;
        ncp[2 * i + 1] = cp[i].second;
    }
}

struct do_get_cts
{
    template <class Graph, class Tree, class PosProp, class BProp, class CMap>
    void operator()(Graph& g, Tree& t, PosProp tpos, BProp beta, CMap cts,
                    bool is_tree, size_t max_depth) const
    {
        vector<size_t> path;
        vector<point_t> cp;
        vector<point_t> ncp;

        for (auto e : edges_range(g))
        {
            auto u = source(e, g);
            auto v = target(e, g);
            if (u == v)
                continue;

            path.clear();
            if (is_tree)
                tree_path(t, u, v, path, max_depth);
            else
                graph_path(t, u, v, path);
            cp.clear();
            get_control_points(path, tpos, beta[e], cp);
            ncp.clear();
            to_bezier(cp, ncp);
            transform(ncp);
            pack(ncp, cts[e]);
        }
    }
};

void get_cts(GraphInterface& gi, GraphInterface& tgi, boost::any otpos,
             boost::any obeta, boost::any octs, bool is_tree, size_t max_depth)
{
    typedef eprop_map_t<vector<double>>::type eprop_t;
    typedef eprop_map_t<double>::type beprop_t;

    eprop_t cts = boost::any_cast<eprop_t>(octs);
    beprop_t beta = boost::any_cast<beprop_t>(obeta);

    gt_dispatch<>()
        (std::bind(do_get_cts(), std::placeholders::_1, std::placeholders::_2,
                   std::placeholders::_3, beta, cts, is_tree, max_depth),
         graph_tool::all_graph_views(),
         graph_tool::always_directed(),
         vertex_scalar_vector_properties())
        (gi.get_graph_view(), tgi.get_graph_view(), otpos);
}


/* Copyright (C) 2012-2020 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/* @file OptimizePermutations.cpp
 * @brief Implementation of optimized permutation networks
 */

#include <cstdlib>
#include <list>
#include <sstream>
#include <memory>

#include <NTL/vector.h>
#include <helib/NumbTh.h>
#include <helib/EncryptedArray.h>
#include <helib/permutations.h>
#include <helib/apiAttributes.h>

namespace helib {

//! \cond FALSE (make doxygen ignore these classes)
template <typename T>
class ClassHash
{ // helper class to make defining hash functions easier
public:
  size_t operator()(const T& t) const { return t.hash(); }
};
//! \endcond

// routines for finding optimal level-collapsing strategies for Benes networks

// Removes duplicates from list x. Uses vector aux as scratch space
//   invariants:
//     * all elements in x lie in a range a..b
//     * aux[a..b] is false before and after removeDups is called
void removeDups(std::list<long>& x, bool* aux)
{
  for (std::list<long>::iterator i = x.begin(); i != x.end();) {
    if (aux[*i])
      i = x.erase(i);
    else {
      aux[*i] = true;
      i++;
    }
  }

  for (std::list<long>::iterator i = x.begin(); i != x.end(); i++)
    aux[*i] = false;
}

// Creates a new list with the old values and old values +/- offset.
// results outside the range -n+1 .. n-1 are discarded
// and all resulting duplicates are removed
void addOffset(std::list<long>& x,
               long offset,
               long n,
               bool* aux,
               UNUSED bool good = false)
{
  for (std::list<long>::iterator i = x.begin(); i != x.end(); i++) {
    long val = *i;
    long val1 = val + offset;
    long val2 = val - offset;
    if (val1 > -n && val1 < n)
      x.push_front(val1);
    if (val2 > -n && val2 < n)
      x.push_front(val2);
    /* FIXME: Alternative Impl:
     *        Replace the above two lines by
     *        if (good) {
     *           if (val1 < 0 && val1 > -n) val1 += n;
     *           if (val2 < 0 && val2 > -n) val2 += n;
     *        }
     *        if (val1 > -n && val1 < n && !aux[val1]) {
     *           x.push_front(val1);
     *           aux[val1] = true;
     *        }
     *        if (val2 > -n && val2 < n && !aux[val2]) {
     *           x.push_front(val2);
     *           aux[val2] = true;
     *        }
     * Then we can eliminate both removeDups and reducedCount
     */
  }

  removeDups(x, aux);
}

// Counts the number of unique elements mod n in x
long reducedCount(const std::list<long>& x, long n, bool* aux)
{
  long res = 0;

  for (std::list<long>::const_iterator i = x.begin(); i != x.end(); i++) {
    long val = *i;
    if (val < 0)
      val += n;

    if (!aux[val]) {
      res++;
      aux[val] = true;
    }
  }

  for (std::list<long>::const_iterator i = x.begin(); i != x.end(); i++) {
    long val = *i;
    if (val < 0)
      val += n;
    aux[val] = false;
  }

  return res;
}

// Compute the cost for all (n choose 2) possible ways to collapse levels.
// For j in [0..nlev-i) tab[i][j] is the cost of collapsing levels i..i+j.
// I.e., how many different shift amounts would we need to implement for
// a permutation-network-layer constructed by collapsing these levels.
void buildBenesCostTable(long n,
                         long k,
                         bool good,
                         NTL::Vec<NTL::Vec<long>>& tab)
{
  long nlev = 2 * k - 1;
  tab.SetLength(nlev);
  for (long i = 0; i < nlev; i++)
    tab[i].SetLength(nlev - i);

  NTL::Vec<bool> aux_vec;
  aux_vec.SetLength(2 * n - 1);
  bool* aux = &aux_vec[n - 1];
  for (long i = 0; i < 2 * n - 1; i++)
    aux_vec[i] = false;

  for (long i = 0; i < nlev; i++) {
    std::list<long> x;

    x.push_front(0L);
    for (long j = 0; j < nlev - i; j++) {
      long shamt = GeneralBenesNetwork::shamt(n, k, i + j);
      // The shift amount for this level

      addOffset(x, shamt, n, aux);
      if (good)
        tab[i][j] = reducedCount(x, n, aux) - 1;
      else
        tab[i][j] = x.size() - 1;
      // FIXME: Alternative Impl:
      //        Replace the 5 lines above by
      //        addOffset(x, shamt, n, aux, good);
      //        tab[i][j] = x.size() - 1;
      //        Also initialize aux to false in every iteration
    }
  }
}

//! \cond FALSE (make doxygen ignore these classes)
class LongNode;
typedef std::shared_ptr<LongNode> LongNodePtr;
// A "shared_ptr" is a pointer with (some) garbage collection

// A LongNode is an implementation of std::list<long>, i.e. a linked list of
// counters, representing a particular way of "collapsing levels" in a Benes
// network. Each LongNode holds a count of collapsed levels, and the sum of all
// counter in the list must equal the number of levels in the Benes network.
class LongNode
{
public:
  long count;       // number of levels collapsed
  LongNodePtr next; // next node in the list

  LongNode(long _count, LongNodePtr _next)
  {
    count = _count;
    next = _next;
  }
};
//! \endcond

// Returns the length of the linked list, starting with this pointer
static long length(LongNodePtr ptr)
{
  long res = 0;
  for (LongNodePtr p = ptr; p != nullptr; p = p->next)
    res++;
  return res;
}

// Converts std::list<long> to NTL::Vec<long>
static long listToVec(NTL::Vec<long>& vec, LongNodePtr ptr)
{
  long len = length(ptr);

  vec.SetLength(len);
  long i = 0;
  for (LongNodePtr p = ptr; p != nullptr; p = p->next) {
    vec[i] = p->count;
    i++;
  }
  return len;
}

// Prints out a list of integers
std::ostream& operator<<(std::ostream& s, LongNodePtr p)
{
  if (p == nullptr)
    return s << "[]";

  s << "[" << p->count;
  for (p = p->next; p != nullptr; p = p->next)
    s << " " << p->count;
  return s << "]";
}

// Data structures to hold the memoization table for the dynamic-programming
// computation of the level collapsing in a Benes network. An entry in the
// table is specified by the first-level index i and the allotted budget
// (depth). Once the computation is over, this entry will contain the optimal
// way of breaking this network (encoded as a LongNode list) and the cost of
// this network.
//! \cond FALSE (make doxygen ignore these classes)
class BenesMemoKey
{
public:
  long i;      // Index of 1st level to consider
  long budget; // max-depth of network after collapsing

  BenesMemoKey(long _i, long _budget)
  {
    i = _i;
    budget = _budget;
  }

  size_t hash() const
  {
    std::stringstream s;
    s << i << " " << budget;
    return std::hash<std::string>()(s.str());
  }

  bool operator==(const BenesMemoKey& other) const
  {
    return i == other.i && budget == other.budget;
  }
};

class BenesMemoEntry
{
public:
  long cost;            // Cost of the optimal solution
  LongNodePtr solution; // The solution itself, as a list of LongNode's

  BenesMemoEntry(long _cost, LongNodePtr _solution)
  {
    cost = _cost;
    solution = _solution;
  }

  BenesMemoEntry() {}
};

typedef std::
    unordered_map<BenesMemoKey, BenesMemoEntry, ClassHash<BenesMemoKey>>
        BenesMemoTable;
//! \endcond

// A dynamic program (implemented as a recursive routine with memoization) for
// computing the optimal collapsing of layers in a generalized Benes network.
//
// The parameter i indicates the sub-problem at hand, namely we are trying
// to find the optimal way of collapsing the sub-network from level i to
// the end (level nlev).
//
// The budget is the largest number of levels that we can afford after
// collapsing. (For example, if budget >= nlev-i, then there is no need to
// collapse anything.)

BenesMemoEntry optimalBenesAux(long i,
                               long budget,
                               long nlev,
                               const NTL::Vec<NTL::Vec<long>>& costTab,
                               BenesMemoTable& memoTab)
{
  assertInRange<InvalidArgument>(i,
                                 0l,
                                 nlev,
                                 "Level to collapse index out of bound",
                                 true);
  assertTrue<InvalidArgument>(budget > 0, "No budget left");

  // Did we already solve this problem? If so just return the solution.
  BenesMemoTable::iterator find = memoTab.find(BenesMemoKey(i, budget));
  if (find != memoTab.end()) {
    return find->second;
  }

  // a new subproblem to process...

  long cost;
  LongNodePtr solution;

  if (i == nlev) { // An empty network, nothing to collapse, trivial solution
    cost = 0;
    solution = LongNodePtr();
  } else if (budget == 1) {
    // Almost no budget is left, the only possible solution is to collapse
    // all remaining levels together.

    cost = costTab[i][nlev - i - 1];
    solution = LongNodePtr(new LongNode(nlev - i, LongNodePtr()));
  } else {
    // We consider collapsing levels i..i+j, for all j in [0..nlev-i),
    // then choose the option that gives the best cost.

    long bestCost = NTL_MAX_LONG;
    long bestJ = 0;
    BenesMemoEntry bestT;
    for (long j = 0; j < nlev - i; j++) {
      // If we collapse levels i..i+j, then we need to compute the optimal
      // way of collapsing the rest, i.e. level i+j+1 to the end.
      BenesMemoEntry t =
          optimalBenesAux(i + j + 1, budget - 1, nlev, costTab, memoTab);

      // The total cost of this solution is the cost of the collapsed level
      // (i..i+j) itself, plus the cost of the optimal solution for the rest.
      if (t.cost + costTab[i][j] < bestCost) {
        bestCost = t.cost + costTab[i][j];
        bestJ = j;
        bestT = t;
      }
    }

    cost = bestCost;
    solution = LongNodePtr(new LongNode(bestJ + 1, bestT.solution));
  }

  return memoTab[BenesMemoKey(i, budget)] = BenesMemoEntry(cost, solution);
}

// Computes an optimal level-collapsing strategy for a Benes network
//   n = the size of the network
//   budget = an upper bound on the number of levels in the collapsed network
//   good = flag indicating whether this is with respect to a "good" generator,
//     for which shifts by i and i-n correspond to the same rotation
//   cost = total number of shifts needed by the collapsed network
//   solution = list indicating how levels in a standard benes network
//      are collapsed: if solution = [s_1 s_2 ... s_k], then k <= budget,
//      and the first s_1 levels are collapsed, the next s_2 levels
//      are collapsed, etc.
void optimalBenes(long n,
                  long budget,
                  bool good,
                  long& cost,
                  LongNodePtr& solution)
{
  long k = GeneralBenesNetwork::depth(n); // k = ceiling(log_2 n)
  long nlev = 2 * k - 1; // before collapsing, we have 2k-1 levels

  NTL::Vec<NTL::Vec<long>> costTab;
  // costTab[i][j] to holds the cost of collapsing levels i..i+j

  buildBenesCostTable(n, k, good, costTab);
  // Compute the cost for all (n choose 2) possible ways to collapse levels.

  BenesMemoTable memoTab;
  BenesMemoEntry t = optimalBenesAux(0, budget, nlev, costTab, memoTab);
  // Compute the optimal collapsing of layers in a width-n Benes network

  cost = t.cost;
  solution = t.solution;
}

/********************************************************************/
/***** Routines for finding the optimal splits among generators *****/
/********************************************************************/

//! \cond FALSE (make doxygen ignore these classes)

// A binary tree data structure for splitting generators
class SplitNode;
typedef std::shared_ptr<SplitNode> SplitNodePtr;
// A "std::shared_ptr" is a pointer with (some) garbage collection

class SplitNode
{
public:
  long order; // order associated with this node
  long mid;   // the "middle" token, 0 or 1
  bool good;  // the "good" flag
  LongNodePtr solution1, solution2;
  // The benes solution(s).
  // For non-middle nodes there may be two different solutions,
  // depending on how the budget is split
  SplitNodePtr left, right; // the children (for internal nodes)

  SplitNode(long _order,
            long _mid,
            bool _good,
            LongNodePtr _solution1,
            LongNodePtr _solution2)
  {
    // constructor for leaves
    order = _order;
    mid = _mid;
    good = _good;
    solution1 = _solution1;
    solution2 = _solution2;
    left = right = SplitNodePtr();
  }

  SplitNode(long _order,
            long _mid,
            bool _good,
            SplitNodePtr _left,
            SplitNodePtr _right)
  {
    // constructor for internal nodes
    order = _order;
    mid = _mid;
    good = _good;
    solution1 = solution2 = LongNodePtr();
    left = _left;
    right = _right;
  }

  bool isLeaf() const { return left == nullptr && right == nullptr; }
};
//! \endcond

// Routines for printing the leaves of a generator-tree
void print(std::ostream& s, SplitNodePtr p, bool first)
{
  if (p->isLeaf()) {
    if (!first)
      s << " ";
    s << "[";
    if (p->mid == 1)
      s << "*";
    if (p->good)
      s << "g ";
    else
      s << "b ";
    s << p->order << " " << p->solution1 << " " << p->solution2 << "]";
  } else {
    print(s, p->left, first);
    print(s, p->right, false);
  }
}
std::ostream& operator<<(std::ostream& s, SplitNodePtr p)
{
  s << "[";
  print(s, p, true);
  s << "]";
  return s;
}

// Data structures to hold the memory table for the dynamic-programming
// computation optimizing a generator tree. An entry in the table is specified
// by (order,good-flag,budget,middle-flag). When the computation is over, this
// entry will contain the optimal tree for a single generator (encoded as a
// SplitNode tree) and the cost of this solution.
//! \cond FALSE (make doxygen ignore these classes)
class LowerMemoKey
{
public:
  long order;  // size of hypercube corresponding to this sub-tree
  bool good;   // is this a good subtree
  long budget; // max-depth of network(s) associated to this node
  long mid;    // whether or not this is the middle layer of the network

  LowerMemoKey(long _order, bool _good, long _budget, long _mid)
  {
    order = _order;
    good = _good;
    budget = _budget;
    mid = _mid;
  }

  size_t hash() const
  {
    std::stringstream s;
    s << order << " " << good << " " << budget << " " << mid;
    return std::hash<std::string>()(s.str());
  }

  bool operator==(const LowerMemoKey& other) const
  {
    return order == other.order && good == other.good &&
           budget == other.budget && mid == other.mid;
  }
};

class LowerMemoEntry
{
public:
  long cost;
  SplitNodePtr solution;

  LowerMemoEntry(long _cost, SplitNodePtr _solution)
  {
    cost = _cost;
    solution = _solution;
  }

  LowerMemoEntry() {}
};

typedef std::
    unordered_map<LowerMemoKey, LowerMemoEntry, ClassHash<LowerMemoKey>>
        LowerMemoTable;

// list structure for managing generators

class GenNode;
typedef std::shared_ptr<GenNode> GenNodePtr;
// A "shared_ptr" is a pointer with (some) garbage collection

class GenNode
{
public:
  SplitNodePtr solution; // the solution tree for this generator
  GenNodePtr next;       // next node in the list

  GenNode(SplitNodePtr _solution, GenNodePtr _next)
  {
    solution = _solution;
    next = _next;
  }
};
//! \endcond

// Compute the length of a list
long length(GenNodePtr ptr)
{
  long res = 0;
  for (GenNodePtr p = ptr; p != nullptr; p = p->next)
    res++;
  return res;
}

std::ostream& operator<<(std::ostream& s, GenNodePtr p)
{
  if (p == nullptr) {
    s << "[]";
    return s;
  }

  s << "[" << p->solution;
  p = p->next;
  while (p != nullptr) {
    s << " " << p->solution;
    p = p->next;
  }
  s << "]";

  return s;
}

// upper level memo table

// Data structures to hold the memory table for optimizing the partition to
// separate generator tree. An entry in the table is specified by the index
// of a generator, the budget allocated to this tree, and the middle flag.
// When the computation is over, this entry will contain the optimal list
// of trees (encoded as a GenNode list) and the cost of this solution.
//! \cond FALSE (make doxygen ignore these classes)
class UpperMemoKey
{
public:
  long i;
  long budget;
  long mid;

  UpperMemoKey(long _i, long _budget, long _mid)
  {
    i = _i;
    budget = _budget;
    mid = _mid;
  }

  size_t hash() const
  {
    std::stringstream s;
    s << i << " " << budget << " " << mid;
    return std::hash<std::string>()(s.str());
  }

  bool operator==(const UpperMemoKey& other) const
  {
    return i == other.i && budget == other.budget && mid == other.mid;
  }
};

class UpperMemoEntry
{
public:
  long cost;
  GenNodePtr solution;

  UpperMemoEntry(long _cost, GenNodePtr _solution)
  {
    cost = _cost;
    solution = _solution;
  }

  UpperMemoEntry() {}
};

typedef std::
    unordered_map<UpperMemoKey, UpperMemoEntry, ClassHash<UpperMemoKey>>
        UpperMemoTable;
//! \endcond

// Optimize a single tree: try all possible ways of splitting the order into
// order1*order2 (and also the solution of not splitting at all). For every
// possible split, try all budget allocations and allocations of good and mid.
LowerMemoEntry optimalLower(long order,
                            bool good,
                            long budget,
                            long mid,
                            LowerMemoTable& lowerMemoTable)
{
  assertTrue<InvalidArgument>(order > 1, "Order must be greater than 1");
  assertTrue<InvalidArgument>(mid == 0 || mid == 1, "mid value is not 1 or 2");
  assertTrue<InvalidArgument>(budget > 0, "No budget left");

  // Did we already solve this problem? If so just return the solution.
  LowerMemoTable::iterator find =
      lowerMemoTable.find(LowerMemoKey(order, good, budget, mid));

  if (find != lowerMemoTable.end()) {
    return find->second;
  }

  long cost;
  SplitNodePtr solution;

  if (mid == 0 && budget == 1) {
    // Insufficient budget, no solution is possible

    cost = NTL_MAX_LONG;
    solution = SplitNodePtr();
  } else {
    // First calculate a solution without splitting, making this a leaf node
    LongNodePtr benesSolution1, benesSolution2;

    if (mid == 1) {
      // this is the middle node, so just one Benes network

      optimalBenes(order, budget, good, cost, benesSolution1);
      benesSolution2 = LongNodePtr();
    } else {
      // not the middle node, so we need two Benes networks.
      // if budget is odd, we split it unevenly

      long cost1, cost2;
      optimalBenes(order, budget / 2, good, cost1, benesSolution1);
      if (budget % 2 == 0) { // both networks have the same budget
        cost2 = cost1;
        benesSolution2 = benesSolution1;
      } else { // one network has budget larger by one than the other
        optimalBenes(order, budget - budget / 2, good, cost2, benesSolution2);
      }

      cost = cost1 + cost2;
    }

    // The initial solution corresponds to this being a leaf node
    solution = SplitNodePtr(
        new SplitNode(order, mid, good, benesSolution1, benesSolution2));

    // Recursively try all the ways of splitting order = order1 * order2

    for (long order1 = 2; order1 < order; order1++) {
      // try all factors of order
      // there is some redundancy here, since we consider
      // both the splits (d, n/d) and (n/d, d); however,
      // we utilize this redundancy (see below) to streamline
      // allocation of the "good" token (if we have it)

      // continue;  // temporary hack to force benes for testing
      if (order % order1 != 0)
        continue;

      bool good1 = good;
      bool good2 = good;

      if (good && NTL::GCD(order1, order / order1) != 1)
        good2 = false;

      // The logic is that if the problem is "good"
      // but the split is not relatively prime,
      // then only one of the subproblems is good.
      // since order1 ranges over all factors of order,
      // it suffices to choose the left subproblem to be
      // the good one.

      // Try all possible ways of splitting the budget between the two nodes
      for (long budget1 = 1; budget1 < budget; budget1++) {

        // A slick way of giving the middle token to one of the
        // nodes if we have it, and to none of the nodes if we don't
        for (long mid1 = 0; mid1 <= mid; mid1++) {
          LowerMemoEntry s1 =
              optimalLower(order1, good1, budget1, mid1, lowerMemoTable);
          // FIXME: If s1.cost==NTL_MAX_LONG we do not need to compute
          //        the cost of s2
          LowerMemoEntry s2 = optimalLower(order / order1,
                                           good2,
                                           budget - budget1,
                                           mid - mid1,
                                           lowerMemoTable);
          if (s1.cost != NTL_MAX_LONG && s2.cost != NTL_MAX_LONG &&
              s1.cost + s2.cost < cost) {
            cost = s1.cost + s2.cost;
            // Record the new best solution
            solution = SplitNodePtr(
                new SplitNode(order, mid, good, s1.solution, s2.solution));
          }
        }
      }
    }
  }

  // Record the best solution in the lowerMemoTable and return it
  return lowerMemoTable[LowerMemoKey(order, good, budget, mid)] =
             LowerMemoEntry(cost, solution);
}

// Optimizing a list of trees, trying all the ways of allocating the budget
// and mid token between the trees. This procedure splits the "current
// remaining budget" between trees i through vec.length()-1.
UpperMemoEntry optimalUpperAux(const NTL::Vec<GenDescriptor>& vec,
                               long i,
                               long budget,
                               long mid,
                               UpperMemoTable& upperMemoTable,
                               LowerMemoTable& lowerMemoTable)
{
  assertInRange<InvalidArgument>(i,
                                 0l,
                                 (long)vec.length(),
                                 "Index i does not point to "
                                 "a tree (index out of range)",
                                 true);
  assertTrue<InvalidArgument>(budget >= 0, "Negative budget");
  assertTrue<InvalidArgument>(mid == 0 || mid == 1, "mid value is not 1 or 2");

  // FIXME: Did we already solve this problem? If so just return the solution.
  UpperMemoTable::iterator find =
      upperMemoTable.find(UpperMemoKey(i, budget, mid));
  if (find != upperMemoTable.end()) {
    return find->second;
  }

  long cost;
  GenNodePtr solution;

  if (i == vec.length()) {
    // An empty list, recursion stops here with trivial solution

    cost = 0;
    solution = GenNodePtr();
  } else if (budget == 0) {
    // recursion stops here with no solution

    cost = NTL_MAX_LONG;
    solution = GenNodePtr();
  } else {
    // allocate resources (budget, mid) between generator i and the rest

    long bestCost = NTL_MAX_LONG;
    LowerMemoEntry bestS;
    UpperMemoEntry bestT;

    for (long budget1 = 1; budget1 <= budget; budget1++) {
      for (long mid1 = 0; mid1 <= mid; mid1++) {
        // Optimize the first tree (index i) with the allotted budget1, mid1
        LowerMemoEntry s = optimalLower(vec[i].order,
                                        vec[i].good,
                                        budget1,
                                        mid1,
                                        lowerMemoTable);
        // FIXME: If s.cost==NTL_MAX_LONG we do not need to compute
        //        the cost of t

        // Optimize the rest of the list with the remaining budget and mid
        UpperMemoEntry t = optimalUpperAux(vec,
                                           i + 1,
                                           budget - budget1,
                                           mid - mid1,
                                           upperMemoTable,
                                           lowerMemoTable);
        if (s.cost != NTL_MAX_LONG && t.cost != NTL_MAX_LONG &&
            s.cost + t.cost < bestCost) {
          bestCost = s.cost + t.cost;
          bestS = s;
          bestT = t;
        }
      }
    }

    cost = bestCost;
    if (cost == NTL_MAX_LONG)
      solution = GenNodePtr();
    else
      solution = GenNodePtr(new GenNode(bestS.solution, bestT.solution));
  }

  // Record the best solution in the upperMemoTable and return it
  return upperMemoTable[UpperMemoKey(i, budget, mid)] =
             UpperMemoEntry(cost, solution);
}

/**
 * optimalUpper is the high-level routine used to find
 * an optimal strategy for evaluating a permutation
 * The inputs are:
 *   * vec: a vector describing the generators, each entry
 *       consist of a long/bool pair (order, good)
 *   * budget: the total budget for levels in the network,
 *       which corresponds to the (multiplicative) circuit depth
 * The outputs are:
 *   * cost: the number of rotations needed (a specific
 *       permutation could use fewer)
 *   * solution: the optimal strategy, described below
 *
 * The solution is a linked list of length equal to the length of vec.
 * The ith entry of the list corresponds to the ith entry in vec.
 * Each entry in the list is a binary tree (SplitNodePtr).
 * Every node in such a tree is either a leaf or an internal node
 *   with two children.
 * Every node contains the order associated with that node, along with
 *   a "mid" flag: 1 means this is a "middle" node, 0 means not.
 * In the top-level list of trees, only one tree root will be a middle node,
 *   and for every internal middle node in any tree, only one child
 *   will be a middle node.
 * A leaf node in a tree will contain two "benes network collapsing solutions",
 *   solution1 and solution2.
 *   If this leaf node is a middle node, solution2 is empty.
 *   Otherwise, there are actually two solutions: this is because
 *   the benes network corresponding to this leaf will appear twice
 *   in the overall network, and if the budget allocated to this leaf
 *   is *odd*, we may want to split the budget unevenly between the two
 *   networks (floor(budget/2) and floor(budget/2)+1.
 *   NOTE: one could consider more general splits of the budget
 *   between these two occurrences of the network, but it seems doubtful
 *   to be of benefit.
 * A "benes network collapsing solution" is a list of longs (longNodePtr).
 *   If the list is [n_1 n_2 ... n_k], this means we are to collapse
 *   the first n_1 levels of the Benes network, then the next n_2 levels,
 *   and so on.  The resulting "collapsed" Benes network will have k
 *   levels.
 *********************************************************************/

// Returns the total number of layers in the corresponding (sub)network
static long recursiveCopy2Tree(OneGeneratorTree& gTree,
                               long nodeIdx,
                               SplitNodePtr& solution)
{
  if (solution->isLeaf()) { // Copy the Benes levels
    return listToVec(gTree[nodeIdx].getData().frstBenes, solution->solution1) +
           listToVec(gTree[nodeIdx].getData().scndBenes, solution->solution2);
  }

  // Copy left and right children into the tree. A child with mid-token
  // is copied last, if none has the mid-token then copy them in order.

  SplitNodePtr leftChld, rightChld;
  if (solution->left->mid) {
    rightChld = solution->left;
    leftChld = solution->right;
  } else {
    leftChld = solution->left;
    rightChld = solution->right;
  }
  SubDimension leftData(leftChld->order, leftChld->good);
  SubDimension rightData(rightChld->order, rightChld->good);
  gTree.addChildren(nodeIdx, leftData, rightData);
  return recursiveCopy2Tree(gTree, gTree.leftChildIdx(nodeIdx), leftChld) +
         recursiveCopy2Tree(gTree, gTree.rightChildIdx(nodeIdx), rightChld);
}

// A recursive procedure for computing the e exponent values
static void computeEvalues(const OneGeneratorTree& T, long idx, long genOrd)
{
  // if either child is missing then we are at a leaf (this is a full tree)
  long left = T[idx].getLeftChild();
  long right = T[idx].getRightChild();
  if (left < 0 || right < 0)
    return;

  SubDimension& lData = (SubDimension&)T[left].getData();
  SubDimension& rData = (SubDimension&)T[right].getData();

  // The entire tree size is sz1 * sz2;
  long sz1 = lData.size; // size of left subtree
  long sz2 = rData.size; // size of right subtree

  long ee = T[idx].getData().e;
  // If right tree is bad, copy e from parent to right and scale left
  if (!rData.good) {
    rData.e = ee;
    lData.e = ee * sz2;
  }
  // If right tree is good and left is bad, copy parent to left and scale right
  else if (!lData.good) {
    lData.e = ee;
    rData.e = ee * sz1;
  }
  // If both are good, scale both using CRT coefficients (assuming GCD==1)
  else {
    long f1 = CRTcoeff(sz1, sz2); // f1 = 0 mod sz1, 1 mod sz2
    long f2 = sz1 * sz2 + 1 - f1; // f2 = 1 mod sz1, 0 mod sz2
    lData.e = NTL::MulMod(ee, f2, genOrd);
    rData.e = NTL::MulMod(ee, f1, genOrd);
  }
  // Recurse on the two subtrees
  computeEvalues(T, left, genOrd);
  computeEvalues(T, right, genOrd);
}

// Copy data from a SplitNode tree to a OneGeneratorTree
static long copyToGenTree(OneGeneratorTree& gTree, SplitNodePtr& solution)
{
  SubDimension rootData(solution->order, solution->good, /*e=*/1);
  gTree.putDataInRoot(rootData);
  long len = recursiveCopy2Tree(gTree, gTree.rootIdx(), solution);
  computeEvalues(gTree, gTree.rootIdx(), solution->order);
  return len;
}

// Compute the trees corresponding to the "optimal" way of breaking
// a permutation into dimensions, subject to some constraints
long GeneratorTrees::buildOptimalTrees(const NTL::Vec<GenDescriptor>& gens,
                                       long depthBound)
{
  // TODO: is this check necessary?
  assertTrue<InvalidArgument>(gens.length() >= 0, "negative gens size");
  trees.SetLength(gens.length()); // allocate space if needed

  if (gens.length() == 0) {
    map2cube.SetLength(1, 0);
    map2array.SetLength(1, 0);
    return 0;
  }
  assertTrue<InvalidArgument>(depthBound > 0, "Zero or negative depthBound");

  // reset the trees, starting from only the roots
  for (long i = 0; i < trees.length(); i++) {
    if (trees[i].getNleaves() > 1) // tree is not empty/trivial
      trees[i].collapseToRoot();
  }

  UpperMemoTable upperMemoTable;
  LowerMemoTable lowerMemoTable;

  // Compute a solution in { t.cost, t.solution }
  UpperMemoEntry t =
      optimalUpperAux(gens, 0, depthBound, 1, upperMemoTable, lowerMemoTable);

  // Copy the solution into the trees
  GenNodePtr midPtr;
  long i = 0, treeIdx = 0, midIdx = 0;
  depth = 0; // Also compute the depth of the permutation network
  for (GenNodePtr genPtr = t.solution; genPtr != nullptr;
       genPtr = genPtr->next, i++) {
    if (genPtr->solution->mid) { // Keep the "middle tree" for last
      midPtr = genPtr;
      midIdx = i;
      continue;
    }
    depth += copyToGenTree(trees[treeIdx], genPtr->solution);
    trees[treeIdx].setAuxKey(gens[i].genIdx);
    treeIdx++;
  }
  if (!midPtr) {
    // no solution, undo initialization and return
    // NTL_MAX_LONG

    depth = 0;
    trees.kill();
    map2cube.kill();
    map2array.kill();
    return NTL_MAX_LONG;
  }

  depth += copyToGenTree(trees[treeIdx], midPtr->solution);
  trees[treeIdx].setAuxKey(gens[midIdx].genIdx);

  // Compute the mapping from array to cube and back
  ComputeCubeMapping();

#ifdef HELIB_DEBUG
  NTL::Vec<long> dims; // The "crude" cube dimensions, one dimension per tree
  getCubeDims(dims);
  std::cerr << " dims=" << dims << std::endl;
  std::cerr << " trees=" << *this << std::endl;
  if (map2cube.length() < 100) {
    std::cerr << " map2cube=" << map2cube << std::endl;
    std::cerr << " map2array=" << map2array << std::endl;
  }
  std::cerr << std::endl;
#endif

  return t.cost;
}

} // namespace helib


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      150409    D. Dirkx          Migrated from personal code.
 *
 *    References
 *      Montebruck O, Gill E. Satellite Orbits, Springer, 2000.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <boost/format.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/oblateSpheroidBodyShapeModel.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/sphericalBodyShapeModel.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_geodetic_coordinate_conversions )

//! Test shape models, for test data, see testGeodeticCoordinateConversions.
BOOST_AUTO_TEST_CASE( testShapeModels )
{
    using namespace tudat::physical_constants;
    using namespace tudat::coordinate_conversions;
    using namespace tudat::unit_conversions;
    using namespace tudat::basic_astrodynamics;

    // Expected Cartesian state, Montenbruck & Gill (2000) Exercise 5.3.
    const Eigen::Vector3d testCartesianPosition( 1917032.190, 6029782.349, -801376.113 );

    // Expected Cartesian state, Montenbruck & Gill (2000) Exercise 5.3.
    const Eigen::Vector3d testGeodeticPosition( -63.667,
                                                convertDegreesToRadians( -7.26654999 ),
                                                convertDegreesToRadians( 72.36312094 ) );

    // Central body characteristics (WGS84 Earth ellipsoid).
    const double flattening = 1.0 / 298.257223563;
    const double equatorialRadius = 6378137.0;

    // Test sphere altitude
    {
        SphericalBodyShapeModel shapeModel = SphericalBodyShapeModel( equatorialRadius );

        // Calculate altitude from shape object.
        const double altitudeFromObject = shapeModel.getAltitude( testCartesianPosition );

        // Calculate object directly.
        const double directAltitude = testCartesianPosition.norm( ) - equatorialRadius;

        // Compare values.
        BOOST_CHECK_EQUAL( altitudeFromObject, directAltitude );
    }

    // Test oblate spheroid
    {
        OblateSpheroidBodyShapeModel shapeModel = OblateSpheroidBodyShapeModel(
                    equatorialRadius, flattening );

        // Calculate altitude from shape object.
        const double altitudeFromObject = shapeModel.getAltitude( testCartesianPosition );

        // Calculate object from free function.
        const double directAltitude = calculateAltitudeOverOblateSpheroid(
                    testCartesianPosition, equatorialRadius, flattening, 1.0E-4 );

        // Compare values.
        BOOST_CHECK_SMALL( altitudeFromObject - testGeodeticPosition.x( ), 1.0E-4 );
        BOOST_CHECK_EQUAL( altitudeFromObject, directAltitude );

        // Test calculation of full geodetic position.
        Eigen::Vector3d calculatedGeodeticPosition = shapeModel.getGeodeticPositionWrtShape(
                    testCartesianPosition );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    calculatedGeodeticPosition, testGeodeticPosition, 1.0E-6 );
    }

    // Test free function altitude calculations
    {
        boost::shared_ptr< OblateSpheroidBodyShapeModel > shapeModel =
                boost::make_shared< OblateSpheroidBodyShapeModel >(
                    equatorialRadius, flattening );

        Eigen::Vector3d bodyPosition =
                ( Eigen::Vector3d( )<<
                  ASTRONOMICAL_UNIT / sqrt( 2.0 ),
                  ASTRONOMICAL_UNIT / sqrt( 2.0 ),
                  ASTRONOMICAL_UNIT * 0.01 ).finished( );
        Eigen::Vector3d inertialTestCartesianPosition = testCartesianPosition + bodyPosition;

        double calculatedAltitute = getAltitudeFromNonBodyFixedPosition(
                    shapeModel, testCartesianPosition, Eigen::Vector3d::Zero( ),
                    Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) );
        BOOST_CHECK_SMALL( calculatedAltitute - testGeodeticPosition.x( ), 1.0E-4 );

        calculatedAltitute = getAltitudeFromNonBodyFixedPosition(
                    shapeModel, inertialTestCartesianPosition, bodyPosition,
                    Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) );
        BOOST_CHECK_SMALL( calculatedAltitute - testGeodeticPosition.x( ), 1.0E-4 );

        Eigen::Quaterniond dummyTestRotation =
                Eigen::AngleAxisd( 0.4343, Eigen::Vector3d::UnitZ( ) ) *
                Eigen::AngleAxisd( 2.4354, Eigen::Vector3d::UnitX( ) ) *
                Eigen::AngleAxisd( 1.2434, Eigen::Vector3d::UnitY( ) );

        calculatedAltitute = getAltitudeFromNonBodyFixedPosition(
                    shapeModel, dummyTestRotation * inertialTestCartesianPosition,
                    dummyTestRotation * bodyPosition,
                    dummyTestRotation.inverse( ) );
        BOOST_CHECK_SMALL( calculatedAltitute - testGeodeticPosition.x( ), 1.0E-4 );

        calculatedAltitute = getAltitudeFromNonBodyFixedPositionFunctions(
                    shapeModel, dummyTestRotation * inertialTestCartesianPosition,
                    boost::lambda::constant(
                        Eigen::Vector3d( dummyTestRotation * bodyPosition ) ),
                    boost::lambda::constant( dummyTestRotation.inverse( ) ) );
        BOOST_CHECK_SMALL( calculatedAltitute - testGeodeticPosition.x( ), 1.0E-4 );

    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/constant/sqrtsmallestposval.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check sqrtsmallestposval behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::sqrtsmallestposval;
  using boost::simd::Sqrtsmallestposval;

  STF_TYPE_IS(decltype(Sqrtsmallestposval<T>()), T);
  STF_EQUAL(Sqrtsmallestposval<T>(), T(1));
  STF_EQUAL(sqrtsmallestposval( as(T{}) ),T(1));
}

STF_CASE_TPL( "Check sqrtsmallestposval behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::sqrtsmallestposval;
  using boost::simd::Sqrtsmallestposval;
  using boost::simd::Smallestposval;

  STF_TYPE_IS(decltype(Sqrtsmallestposval<T>()), T);
  auto z1 = Sqrtsmallestposval<T>();
  STF_ULP_EQUAL(z1*z1, Smallestposval<T>(), 0.5);
  auto z2 = sqrtsmallestposval( as(T{}) );
  STF_ULP_EQUAL(z2*z2, Smallestposval<T>(), 0.5);

}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <Eigen/Dense>
#include <iostream>

#include <boost/random/mersenne_twister.hpp>
#include <boost/shared_ptr.hpp>

#include "kmeans.hpp"

using namespace Eigen;
using std::cout;
using std::endl;

template<class T>
class DPMeans : public KMeans<T>
{
public:
  DPMeans(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx, uint32_t K0,
    double lambda, boost::mt19937* pRndGen);
  virtual ~DPMeans();

//  void initialize(const Matrix<T,Dynamic,Dynamic>& x);

  virtual void updateLabels();
  virtual void updateCenters();

  virtual uint32_t indOfClosestCluster(int32_t i);

protected:
  double lambda_;
};

// -------------------------------- impl ----------------------------------
template<class T>
DPMeans<T>::DPMeans(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx,
    uint32_t K0, double lambda, boost::mt19937* pRndGen)
  : KMeans<T>(spx,K0,pRndGen), lambda_(lambda)
{}

template<class T>
DPMeans<T>::~DPMeans()
{}

template<class T>
uint32_t DPMeans<T>::indOfClosestCluster(int32_t i)
{
  int z_i = this->K_;
  T sim_closest = lambda_;
  for (uint32_t k=0; k<this->K_; ++k)
  {
    T sim_k = this->dist(this->ps_.col(k), this->spx_->col(i));
    if(this->closer(sim_k, sim_closest))
    {
      sim_closest = sim_k;
      z_i = k;
    }
  }
  return z_i;
}

template<class T>
void DPMeans<T>::updateLabels()
{
//#pragma omp parallel for
// TODO not sure how to parallelize
  for(uint32_t i=0; i<this->N_; ++i)
  {
    uint32_t z_i = indOfClosestCluster(i);

    if(z_i == this->K_)
    {
      this->ps_.conservativeResize(this->D_,this->K_+1);
      this->Ns_.conservativeResize(this->K_+1);
      this->ps_.col(this->K_) = this->spx_->col(i);
      this->Ns_(this->K_) = 1.;
      this->K_ ++;
    }
    this->z_(i) = z_i;
  }
};

template<class T>
void DPMeans<T>::updateCenters()
{
  vector<bool> toDelete(this->K_,false);
#pragma omp parallel for
  for(uint32_t k=0; k<this->K_; ++k)
  {
    this->ps_.col(k) = this->computeCenter(k);
    if (this->Ns_(k) <= 0)
      toDelete[k] = true;
  }

  uint32_t kNew = this->K_;
  for(int32_t k=this->K_-1; k>-1; --k)
    if(toDelete[k])
    {
      cout<<"cluster k "<<k<<" empty"<<endl;
#pragma omp parallel for
      for(uint32_t i=0; i<this->N_; ++i)
      {
        if(static_cast<int32_t>(this->z_(i)) >= k) this->z_(i)--;
      }
      kNew --;
    }

  Matrix<T,Dynamic,Dynamic> psNew(this->D_,kNew);
  int32_t offset = 0;
  for(uint32_t k=0; k<this->K_; ++k)
    if(toDelete[k])
    {
      offset ++;
    }else{
      psNew.col(k-offset) = this->ps_.col(k);
    }
  this->ps_ = psNew;
  this->K_ = kNew;
};


/*
 * Copyright 2020 INRIA
 */

#ifndef __eigenpy_decomposition_eigen_solver_hpp__
#define __eigenpy_decomposition_eigen_solver_hpp__

#include "eigenpy/eigenpy.hpp"
#include "eigenpy/eigen-to-python.hpp"

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include "eigenpy/utils/scalar-name.hpp"

namespace eigenpy
{

  template<typename _MatrixType>
  struct EigenSolverVisitor
  : public boost::python::def_visitor< EigenSolverVisitor<_MatrixType> >
  {

    typedef _MatrixType MatrixType;
    typedef typename MatrixType::Scalar Scalar;
    typedef Eigen::EigenSolver<MatrixType> Solver;

    template<class PyClass>
    void visit(PyClass& cl) const
    {
      namespace bp = boost::python;
      cl
      .def(bp::init<>("Default constructor"))
      .def(bp::init<Eigen::DenseIndex>(bp::arg("size"),
                                       "Default constructor with memory preallocation"))
      .def(bp::init<MatrixType,bp::optional<bool> >(bp::args("matrix","compute_eigen_vectors"),
                                                    "Computes eigendecomposition of given matrix"))

      .def("eigenvalues",&Solver::eigenvalues,bp::arg("self"),
           "Returns the eigenvalues of given matrix.",
           bp::return_internal_reference<>())
      .def("eigenvectors",&Solver::eigenvectors,bp::arg("self"),
           "Returns the eigenvectors of given matrix.")

      .def("compute",&EigenSolverVisitor::compute_proxy<MatrixType>,
           bp::args("self","matrix"),
           "Computes the eigendecomposition of given matrix.",
           bp::return_self<>())
      .def("compute",(Solver & (Solver::*)(const Eigen::EigenBase<MatrixType> & matrix, bool))&Solver::compute,
           bp::args("self","matrix","compute_eigen_vectors"),
           "Computes the eigendecomposition of given matrix.",
           bp::return_self<>())

      .def("getMaxIterations",&Solver::getMaxIterations,bp::arg("self"),
           "Returns the maximum number of iterations.")
      .def("setMaxIterations",&Solver::setMaxIterations,bp::args("self","max_iter"),
           "Sets the maximum number of iterations allowed.",
           bp::return_self<>())

      .def("pseudoEigenvalueMatrix",&Solver::pseudoEigenvalueMatrix,bp::arg("self"),
           "Returns the block-diagonal matrix in the pseudo-eigendecomposition.")
      .def("pseudoEigenvectors",&Solver::pseudoEigenvectors ,bp::arg("self"),
           "Returns the pseudo-eigenvectors of given matrix.",
           bp::return_internal_reference<>())

      .def("info",&Solver::info,bp::arg("self"),
           "NumericalIssue if the input contains INF or NaN values or overflow occured. Returns Success otherwise.")
      ;
    }

    static void expose()
    {
      static const std::string classname = "EigenSolver" + scalar_name<Scalar>::shortname();
      expose(classname);
    }

    static void expose(const std::string & name)
    {
      namespace bp = boost::python;
      bp::class_<Solver>(name.c_str(),
                         bp::no_init)
      .def(EigenSolverVisitor());
    }

  private:

    template<typename MatrixType>
    static Solver & compute_proxy(Solver & self, const Eigen::EigenBase<MatrixType> & matrix)
    {
      return self.compute(matrix);
    }

  };

} // namespace eigenpy

#endif // ifndef __eigenpy_decomposition_eigen_solver_hpp__


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       David Salinas
 *
 *    Copyright (C) 2014 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <fstream>
#include <sstream>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "skeleton_blocker_simplifiable"
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <gudhi/Skeleton_blocker.h>

template<typename ComplexType> class Skeleton_blocker_sub_complex;
typedef Gudhi::skeleton_blocker::Skeleton_blocker_simple_traits Traits;
typedef Gudhi::skeleton_blocker::Skeleton_blocker_complex<Traits> Complex;
typedef Complex::Vertex_handle Vertex_handle;
typedef Complex::Root_vertex_handle Root_vertex_handle;
typedef Gudhi::skeleton_blocker::Skeleton_blocker_simplex<Vertex_handle> Simplex;

void build_complete(int n, Complex& complex) {
  complex.clear();
  for (int i = 0; i < n; i++)
    complex.add_vertex();
  for (int i = 0; i < n; i++)
    for (int j = 0; j < i; j++)
      complex.add_edge_without_blockers(Vertex_handle(i), Vertex_handle(j));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_simplifiable_contraction1) {
  enum { a, b, x, y, z, n };
  Complex complex(n);
  build_complete(n, complex);
  complex.remove_edge(static_cast<Vertex_handle> (b), static_cast<Vertex_handle> (z));
  complex.add_blocker(Simplex(static_cast<Vertex_handle> (a), static_cast<Vertex_handle> (x),
                              static_cast<Vertex_handle> (y)));
  complex.add_blocker(Simplex(static_cast<Vertex_handle> (b), static_cast<Vertex_handle> (x),
                              static_cast<Vertex_handle> (y)));

  // Print result
  std::clog << "complex before complex" << complex.to_string() << std::endl;

  std::clog << std::endl << std::endl;
  complex.contract_edge(static_cast<Vertex_handle> (a), static_cast<Vertex_handle> (b));
  // Print result
  std::clog << "ContractEdge(0,1)\n";
  PRINT(complex.to_string());

  // verification
  for (int i = 0; i < 5; i++)
    if (i != 1) BOOST_CHECK(complex.contains(Simplex(static_cast<Vertex_handle> (i))));
  BOOST_CHECK(!complex.contains_edge(static_cast<Vertex_handle> (a), static_cast<Vertex_handle> (b)));

  BOOST_CHECK(complex.contains_blocker(Simplex(*complex.get_address(Root_vertex_handle(a)),
                                               *complex.get_address(Root_vertex_handle(x)),
                                               *complex.get_address(Root_vertex_handle(y)))));

  BOOST_CHECK(complex.num_edges() == 6);
  BOOST_CHECK(complex.num_blockers() == 1);
  Simplex sigma;
  sigma.add_vertex(static_cast<Vertex_handle> (a));
  sigma.add_vertex(static_cast<Vertex_handle> (x));
  sigma.add_vertex(static_cast<Vertex_handle> (y));
  sigma.add_vertex(static_cast<Vertex_handle> (z));
  BOOST_CHECK(!(complex.contains(sigma)));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_simplifiable_contraction2) {
  enum { a, b, x, y, z, n };
  Complex complex(n);
  build_complete(n, complex);
  complex.remove_edge(static_cast<Vertex_handle> (b), static_cast<Vertex_handle> (x));
  Simplex blocker;
  blocker.add_vertex(static_cast<Vertex_handle> (a));
  blocker.add_vertex(static_cast<Vertex_handle> (y));
  blocker.add_vertex(static_cast<Vertex_handle> (z));

  complex.add_blocker(blocker);

  // Print result
  std::clog << "complex complex" << complex.to_string();
  std::clog << std::endl << std::endl;
  complex.contract_edge(static_cast<Vertex_handle> (a), static_cast<Vertex_handle> (b));

  std::clog << "complex.ContractEdge(a,b)" << complex.to_string();

  std::clog << std::endl << std::endl;

  // there should be one blocker (a,c,d,e) in the complex
  BOOST_CHECK(complex.contains_blocker(Simplex(static_cast<Vertex_handle> (a), static_cast<Vertex_handle> (x),
                                               static_cast<Vertex_handle> (y), static_cast<Vertex_handle> (z))));
  BOOST_CHECK(complex.num_blockers() == 1);
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_simplifiable_link_condition1) {
  Complex complex(0);
  // Build the complexes
  build_complete(4, complex);
  complex.add_blocker(Simplex(static_cast<Vertex_handle> (0), static_cast<Vertex_handle> (1), static_cast<Vertex_handle> (2)));

  // Print result
  std::clog << "complex complex" << complex.to_string();
  std::clog << std::endl << std::endl;

  BOOST_CHECK(complex.link_condition(Vertex_handle(1), Vertex_handle(2), true));

  BOOST_CHECK(!complex.link_condition(Vertex_handle(1), Vertex_handle(2), false));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_simplifiable_collapse0) {
  Complex complex(5);
  build_complete(4, complex);
  complex.add_vertex();
  complex.add_edge_without_blockers(static_cast<Vertex_handle> (2), static_cast<Vertex_handle> (4));
  complex.add_edge_without_blockers(static_cast<Vertex_handle> (3), static_cast<Vertex_handle> (4));
  // Print result
  std::clog << "initial complex :\n" << complex.to_string();
  std::clog << std::endl << std::endl;

  Simplex simplex_123(static_cast<Vertex_handle> (1), static_cast<Vertex_handle> (2), static_cast<Vertex_handle> (3));
  complex.remove_star(simplex_123);
  std::clog << "complex.remove_star(1,2,3):\n" << complex.to_string();
  std::clog << std::endl << std::endl;

  // verification
  BOOST_CHECK(complex.contains_blocker(simplex_123));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_simplifiable_collapse1) {
  Complex complex(5);
  build_complete(4, complex);
  complex.add_blocker(Simplex(Vertex_handle(0), Vertex_handle(1), Vertex_handle(2), Vertex_handle(3)));
  // Print result
  std::clog << "initial complex :\n" << complex.to_string();
  std::clog << std::endl << std::endl;

  Simplex simplex_123(Vertex_handle(1), Vertex_handle(2), Vertex_handle(3));
  complex.remove_star(simplex_123);
  std::clog << "complex.remove_star(1,2,3):\n" << complex.to_string();
  std::clog << std::endl << std::endl;

  // verification
  BOOST_CHECK(complex.contains_blocker(simplex_123));
  BOOST_CHECK(complex.num_blockers() == 1);
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_simplifiable_collapse2) {
  Complex complex(5);
  build_complete(4, complex);
  complex.add_vertex();
  complex.add_edge_without_blockers(Vertex_handle(1), Vertex_handle(4));
  complex.add_edge_without_blockers(Vertex_handle(2), Vertex_handle(4));
  complex.add_edge_without_blockers(Vertex_handle(3), Vertex_handle(4));
  complex.add_blocker(Simplex(Vertex_handle(1), Vertex_handle(2), Vertex_handle(3), Vertex_handle(4)));
  // Print result
  std::clog << "initial complex :\n" << complex.to_string();
  std::clog << std::endl << std::endl;

  Simplex sigma(Vertex_handle(1), Vertex_handle(2), Vertex_handle(3));
  complex.remove_star(sigma);
  std::clog << "complex.remove_star(1,2,3):\n" << complex.to_string();
  std::clog << std::endl << std::endl;

  // verification
  BOOST_CHECK(!complex.contains_blocker(Simplex(Vertex_handle(1), Vertex_handle(2),
                                                Vertex_handle(3), Vertex_handle(4))));
  BOOST_CHECK(complex.contains_blocker(sigma));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_simplifiable_collapse3) {
  Complex complex(5);
  build_complete(4, complex);
  complex.add_vertex();
  complex.add_edge_without_blockers(Vertex_handle(1), Vertex_handle(4));
  complex.add_edge_without_blockers(Vertex_handle(2), Vertex_handle(4));
  complex.add_edge_without_blockers(Vertex_handle(3), Vertex_handle(4));
  complex.add_blocker(Simplex(Vertex_handle(1), Vertex_handle(2), Vertex_handle(3), Vertex_handle(4)));
  // Print result
  std::clog << "initial complex:\n" << complex.to_string();
  std::clog << std::endl << std::endl;

  complex.remove_star(static_cast<Vertex_handle> (2));
  std::clog << "complex after remove star of 2:\n" << complex.to_string();

  BOOST_CHECK(complex.contains_blocker(Simplex(Vertex_handle(1), Vertex_handle(3), Vertex_handle(4))));
  BOOST_CHECK(!complex.contains_blocker(Simplex(Vertex_handle(1), Vertex_handle(2),
                                                Vertex_handle(3), Vertex_handle(4))));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_simplifiable_add_simplex) {
  Complex complex(4);
  build_complete(4, complex);
  complex.add_blocker(Simplex(Vertex_handle(0), Vertex_handle(1), Vertex_handle(3)));
  std::clog << "initial complex:\n" << complex.to_string();
  std::clog << std::endl << std::endl;

  complex.add_simplex(Simplex(Vertex_handle(0), Vertex_handle(1), Vertex_handle(3)));
  std::clog << "complex after add_simplex:\n" << complex.to_string();
  BOOST_CHECK(complex.num_blockers() == 1);
  BOOST_CHECK(complex.contains_blocker(Simplex(Vertex_handle(0), Vertex_handle(1),
                                               Vertex_handle(2), Vertex_handle(3))));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_simplifiable_add_simplex2) {
  Complex complex;
  build_complete(4, complex);
  // Print result
  std::clog << "initial complex:\n" << complex.to_string();
  std::clog << std::endl << std::endl;

  Complex copy(complex.num_vertices());

  std::vector<Simplex> simplices(complex.complex_simplex_range().begin(), complex.complex_simplex_range().end());
  sort(simplices.begin(), simplices.end(), [&](const Simplex& s1, const Simplex & s2) {
    return s1.dimension() < s2.dimension();
  });
  for (const auto & simplex : simplices) {
    if (!copy.contains(simplex) && simplex.dimension() == 1)
      copy.add_edge_without_blockers(simplex.first_vertex(), simplex.last_vertex());
    if (!copy.contains(simplex) && simplex.dimension() > 1)
      copy.add_simplex(simplex);
  }

  std::clog << "complex after add_simplex:\n" << copy.to_string();

  BOOST_CHECK(complex.num_blockers() == copy.num_blockers());
  BOOST_CHECK(complex.num_edges() == copy.num_edges());
  BOOST_CHECK(complex.num_vertices() == copy.num_vertices());
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_simplifiable_add_simplex3) {
  Complex complex(5);
  build_complete(5, complex);
  complex.remove_edge(Vertex_handle(3), Vertex_handle(4));
  Simplex sigma(Vertex_handle(0), Vertex_handle(1), Vertex_handle(2));
  complex.add_blocker(sigma);
  // Print result
  std::clog << "initial complex:\n" << complex.to_string();
  std::clog << std::endl << std::endl;
  complex.add_simplex(sigma);
  //should create two blockers 0123 and 0124
  std::clog << "complex after adding simplex 012:\n" << complex.to_string();
  BOOST_CHECK(complex.num_blockers() == 2);
  BOOST_CHECK(complex.contains_blocker(Simplex(Vertex_handle(0), Vertex_handle(1),
                                               Vertex_handle(2), Vertex_handle(3))));
  BOOST_CHECK(complex.contains_blocker(Simplex(Vertex_handle(0), Vertex_handle(1),
                                               Vertex_handle(2), Vertex_handle(4))));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_simplifiable_add_simplex4) {
  int n = 6;
  Complex complex(n);

  // add all simplex 0..n without i
  for (int i = 0; i < n; i++) {
    Simplex s;
    for (int k = 0; k < n; k++)
      s.add_vertex(Vertex_handle(k));
    s.remove_vertex(Vertex_handle(i));
    complex.add_simplex(s);

    //at step i there is only blocker 0..i
    BOOST_CHECK(!(i < 2 && complex.num_blockers() > 0));
    if (i >= 2 && complex.num_blockers() != 1) {
      Simplex b;
      for (int k = 0; k < i; k++)
        b.add_vertex(Vertex_handle(i));
      BOOST_CHECK(complex.contains_blocker(b));
    }
  }
  Simplex s;
  for (int k = 0; k < n; k++)
    s.add_vertex(Vertex_handle(k));
  BOOST_CHECK(complex.num_blockers() == 1);
  BOOST_CHECK(complex.contains_blocker(s));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_simplifiable_add_edge) {
  Complex complex(4);
  for (unsigned i = 0u; i < 4; i++)
    complex.add_edge(Vertex_handle(i), Vertex_handle((i + 1) % 4));

  // Print result
  std::clog << "initial complex:\n" << complex.to_string();
  std::clog << std::endl << std::endl;
  complex.add_edge(Vertex_handle(1), Vertex_handle(3));
  //should create two blockers 013 and 012
  std::clog << "complex after adding edge 13:\n" << complex.to_string();
  BOOST_CHECK(complex.num_blockers() == 2);
  BOOST_CHECK(complex.contains_blocker(Simplex(Vertex_handle(0), Vertex_handle(1), Vertex_handle(3))));
  BOOST_CHECK(complex.contains_blocker(Simplex(Vertex_handle(1), Vertex_handle(2), Vertex_handle(3))));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_simplifiable_remove_popable_blockers) {
  Complex complex;
  build_complete(4, complex);
  complex.add_vertex();
  complex.add_edge_without_blockers(Vertex_handle(3), Vertex_handle(4));
  complex.add_edge_without_blockers(Vertex_handle(2), Vertex_handle(4));
  Simplex sigma1 = Simplex(Vertex_handle(1), Vertex_handle(2), Vertex_handle(3));
  Simplex sigma2 = Simplex(Vertex_handle(2), Vertex_handle(3), Vertex_handle(4));

  complex.add_blocker(sigma1);
  complex.add_blocker(sigma2);
  std::clog << "complex complex" << complex.to_string();
  std::clog << std::endl << std::endl;
  std::clog << "complex.RemovePopableBlockers();" << std::endl;
  complex.remove_popable_blockers();
  std::clog << "complex complex" << complex.to_string();
  std::clog << std::endl << std::endl;

  BOOST_CHECK(complex.num_blockers() == 1);

  // test 2
  complex.clear();
  build_complete(4, complex);
  complex.add_vertex();
  complex.add_vertex();
  complex.add_edge_without_blockers(Vertex_handle(3), Vertex_handle(4));
  complex.add_edge_without_blockers(Vertex_handle(2), Vertex_handle(4));
  complex.add_edge_without_blockers(Vertex_handle(2), Vertex_handle(5));
  complex.add_edge_without_blockers(Vertex_handle(3), Vertex_handle(5));
  complex.add_edge_without_blockers(Vertex_handle(4), Vertex_handle(5));
  sigma1 = Simplex(Vertex_handle(1), Vertex_handle(2), Vertex_handle(3));
  sigma2 = Simplex(Vertex_handle(2), Vertex_handle(3), Vertex_handle(4));

  complex.add_blocker(sigma1);
  complex.add_blocker(sigma2);
  std::clog << "complex complex" << complex.to_string();
  std::clog << std::endl << std::endl;
  std::clog << "complex.RemovePopableBlockers();" << std::endl;
  complex.remove_popable_blockers();
  std::clog << "complex complex" << complex.to_string();

  std::clog << std::endl << std::endl;
  BOOST_CHECK(complex.num_blockers() == 0);
}


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/constant/maxflint.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check maxflint behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxflint;
  using boost::simd::Maxflint;

  STF_TYPE_IS(decltype(Maxflint<T>()), T);
  STF_EQUAL(Maxflint<T>(), T(1));
  STF_EQUAL(maxflint( as(T{}) ),T(1));
}

STF_CASE_TPL( "Check maxflint behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxflint;
  using boost::simd::Maxflint;

  STF_TYPE_IS(decltype(Maxflint<T>()), T);
  STF_IEEE_EQUAL(Maxflint<T>(), 9007199254740992.0);
  STF_IEEE_EQUAL(maxflint( as(T{}) ), 9007199254740992.0);
}
STF_CASE_TPL( "Check maxflint behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxflint;
  using boost::simd::Maxflint;

  STF_TYPE_IS(decltype(Maxflint<T>()), T);
  STF_IEEE_EQUAL(Maxflint<T>(), 16777216.0f);
  STF_IEEE_EQUAL(maxflint( as(T{}) ), 16777216.0f);
}


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

#ifndef STAPL_RUNTIME_UTILITY_TREE_HPP
#define STAPL_RUNTIME_UTILITY_TREE_HPP

#include "algorithm.hpp"
#include <cmath>
#include <tuple>
#include <utility>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/range/irange.hpp>
#include <boost/range/iterator_range.hpp>

namespace stapl {

namespace runtime {

//////////////////////////////////////////////////////////////////////
/// @brief Returns the root, parent and children ids for the part of a flat tree
///        that @p myid belongs to.
///
/// @tparam IntType Integral type.
///
/// @param myid Id for which the part of the tree will be generated.
/// @param n    Total number of ids.
///
/// @return A tuple with the root, parent and children ids as a range.
///
/// @ingroup runtimeUtility
//////////////////////////////////////////////////////////////////////
template<typename IntType, typename Size>
std::tuple<IntType, IntType, decltype(boost::irange(std::declval<IntType>(),
                                                    std::declval<IntType>()))>
make_flat_tree(IntType myid, Size n)
{
  return (myid !=0 ? std::make_tuple(IntType(0),
                                     IntType(0),
                                     boost::irange(IntType(0), IntType(0)))
                   : std::make_tuple(IntType(0),
                                     IntType(0),
                                     boost::irange(IntType(1), IntType(n))));
}

//////////////////////////////////////////////////////////////////////
/// @brief Returns the root, parent and children ids for the part of a binary
///        tree that @p myid belongs to.
///
/// @tparam IntType Integral type.
///
/// @param myid Id for which the part of the tree will be generated.
/// @param n    Total number of ids.
///
/// @return A tuple with the root, parent and children ids.
///
/// @ingroup runtimeUtility
//////////////////////////////////////////////////////////////////////
template<typename IntType, typename Size>
std::tuple<IntType, IntType, decltype(boost::irange(std::declval<IntType>(),
                                                    std::declval<IntType>()))>
make_binary_tree(IntType myid, Size n)
{
  const IntType parent = ((myid==0) ? 0 : ((myid-1)/2));
  const Size left      = (2*(myid+1) - 1);
  const Size right     = (2*(myid+1));
  if (left<n) {
    if (right<n) {
      return std::make_tuple(IntType(0),
                             parent,
                             boost::irange(IntType(left), IntType(right + 1)));
    }
    else {
      return std::make_tuple(IntType(0),
                             parent,
                             boost::irange(IntType(left), IntType(left + 1)));
    }
  }
  return std::make_tuple(IntType(0),
                         parent,
                         boost::irange(IntType(0), IntType(0)));

}

//////////////////////////////////////////////////////////////////////
/// @brief Iterator to generate range of children ids required from
///        @ref make_binomial_tree.
///
/// @tparam IntType Integral type.
/// @tparam Size    Size type.
///
/// @ingroup runtimeUtility
//////////////////////////////////////////////////////////////////////
template<typename IntType, typename Size>
class binomial_tree_iterator
: public boost::iterator_facade<
           binomial_tree_iterator<IntType, Size>,
           IntType,
           boost::forward_traversal_tag,
           IntType>
{
private:
  friend class boost::iterator_core_access;

  IntType m_id;
  Size    m_idx;

public:
  constexpr binomial_tree_iterator(void) noexcept
  : m_id(),
    m_idx(0)
  { }

  constexpr binomial_tree_iterator(const IntType id, const Size index) noexcept
  : m_id(id),
    m_idx(index)
  { }

private:
  IntType dereference(void) const
  { return (m_id + m_idx); }

  bool equal(binomial_tree_iterator const& other) const noexcept
  { return (m_idx==other.m_idx); }

  void increment(void) noexcept
  { m_idx /= 2; }
};

//////////////////////////////////////////////////////////////////////
/// @brief Returns the root, parent and children ids for the part of a binomial
///        tree that @p myid belongs to.
///
/// @tparam IntType Integral type.
///
/// @param myid Id for which the part of the tree will be generated.
/// @param n    Total number of ids.
///
/// @return A tuple with the root, parent and children ids.
///
/// @ingroup runtimeUtility
//////////////////////////////////////////////////////////////////////
template<typename IntType, typename Size>
std::tuple<IntType,
           IntType,
           boost::iterator_range<binomial_tree_iterator<IntType, Size>>>
make_binomial_tree(IntType myid, Size n)
{
  using iterator_type = binomial_tree_iterator<IntType, Size>;

  // round up to the next highest power of 2
  const Size N2 = (0x1 << integral_ceil_log2(n));

  // find level in the tree and parent
  IntType parent    = 0;
  Size level        = 1;
  IntType tmpParent = 0;
  for (Size tmp = N2/(0x1<<level); tmpParent!=myid; ++level, tmp /= 2) {
    if (myid>=(tmpParent+tmp)) {
      parent     = tmpParent;
      tmpParent += tmp;
    }
  }

  // find the first valid child id
  for (Size first = N2/(0x1<<level); first >= 1; first /= 2) {
    const auto t = (myid + first);
    if (t < n)
      return std::make_tuple(IntType(0),
                             parent,
                             boost::make_iterator_range(
                               iterator_type{myid, first}, iterator_type{}));
  }

  // has no children
  return std::make_tuple(IntType(0),
                         parent,
                         boost::make_iterator_range(
                           iterator_type{}, iterator_type{}));
}

} // namespace runtime

} // namespace stapl

#endif


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110901    L. van der Ham    File created.
 *      120221    L. van der Ham    Fixed bug and up to date with latest standards.
 *      120227    K. Kumar          Boostified unit test; renamed file.
 *      120307    K. Kumar          Moved file.
 *
 *    References
 *        Wakker, K.F., "Astrodynamics I, AE4-874", Delft University of Technology, 2007.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/Gravitation/jacobiEnergy.h"

namespace tudat
{
namespace unit_tests
{

//! Test if Jacobi energy is computed correctly.
BOOST_AUTO_TEST_CASE( testJacobiEnergy )
{
    // Test 1: test Jacobi energy at L1.
    {
        namespace crtbp = gravitation::circular_restricted_three_body_problem;

        // Set mass parameter for Earth-moon system. Value from Table 3.1 (Wakker, 2007).
        double massParameter = 0.01215;

        // Initialize position L1, from Table 3.4 (Wakker, 2007).
        Eigen::VectorXd stateAtL1 = Eigen::VectorXd::Zero( 6 );
        stateAtL1( crtbp::xCartesianPositionIndex ) = 0.836914;

        // Set expected value of Jacobi energy at L1.
        double expectedJacobiEnergy = 3.1883;

        // Compute Jacobi energy.
        double computedJacobiEnergy = crtbp::computeJacobiEnergy( massParameter, stateAtL1 );

        // Check if expected Jacobi energy matches computed.
        BOOST_CHECK_CLOSE_FRACTION( expectedJacobiEnergy,  computedJacobiEnergy, 1.0e-4 );
    }

    // Test 2: test Jacobi energy at L4.
    {
        namespace crtbp = gravitation::circular_restricted_three_body_problem;

        // Set mass parameter for Earth-moon system. Value from Table 3.1 (Wakker, 2007).
        double massParameter = 0.01215;

        // Initialize position L4, from Table 3.4 (Wakker, 2007).
        Eigen::VectorXd stateAtL4 = Eigen::VectorXd::Zero( 6 );
        stateAtL4( crtbp::xCartesianPositionIndex ) = 0.487849;
        stateAtL4( crtbp::yCartesianPositionIndex ) = 0.866025;

        // Set expected value of Jacobi energy at L4.
        double expectedJacobiEnergy = 2.9880;

        // Compute Jacobi energy.
        double computedJacobiEnergy = crtbp::computeJacobiEnergy( massParameter, stateAtL4 );

        // Check if expected Jacobi energy matches computed.
        BOOST_CHECK_CLOSE_FRACTION( expectedJacobiEnergy,  computedJacobiEnergy, 1.0e-6 );
    }
}

} // namespace unit_tests
} // namespace tudat


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This example shows how to do semantic segmentation on an image using net pretrained
    on the PASCAL VOC2012 dataset.  For an introduction to what segmentation is, see the
    accompanying header file dnn_semantic_segmentation_ex.h.

    Instructions how to run the example:
    1. Download the PASCAL VOC2012 data, and untar it somewhere.
       http://host.robots.ox.ac.uk/pascal/VOC/voc2012/VOCtrainval_11-May-2012.tar
    2. Build the dnn_semantic_segmentation_train_ex example program.
    3. Run:
       ./dnn_semantic_segmentation_train_ex /path/to/VOC2012
    4. Wait while the network is being trained.
    5. Build the dnn_semantic_segmentation_ex example program.
    6. Run:
       ./dnn_semantic_segmentation_ex /path/to/VOC2012-or-other-images

    An alternative to steps 2-4 above is to download a pre-trained network
    from here: http://dlib.net/files/semantic_segmentation_voc2012net.dnn

    It would be a good idea to become familiar with dlib's DNN tooling before reading this
    example.  So you should read dnn_introduction_ex.cpp and dnn_introduction2_ex.cpp
    before reading this example program.
*/

#include "dnn_semantic_segmentation_ex.h"

#include "dnn_semantic_segmentation_net.h"

#include <iostream>
#include <dlib/data_io.h>
#include <dlib/gui_widgets.h>
#include <dlib/matrix.h>

using namespace std;
using namespace dlib;

// Read the file containing the trained network from the working directory.
anet_type net;

void load_net(string path){
    //"semantic_face_net.dnn"
    deserialize(path) >> net;
}

matrix<uint16_t> infer_net(dlib::matrix<rgb_pixel> input_image){
        return net(input_image);
}


/*
 *  MIT License
 *
 *  Copyright (c) 2017 Piotr Dobrowolski
 *
 *  permission is hereby granted, free of charge, to any person obtaining a copy
 *  of this software and associated documentation files (the "Software"), to deal
 *  in the Software without restriction, including without limitation the rights
 *  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *  copies of the Software, and to permit persons to whom the Software is
 *  furnished to do so, subject to the following conditions:
 *
 *  The above copyright notice and this permission notice shall be included in all
 *  copies or substantial portions of the Software.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 *  SOFTWARE.
 *
 */
/*
 * Author: Piotr Dobrowolski
 * dobrypd[at]gmail[dot]com
 *
 */
#include <gtest/gtest.h>
#include <boost/assert.hpp>
#include <iostream>
#include <opencv2/opencv.hpp>

#include "kernel/GenericFrame.hpp"
#include "testArgs.hpp"

using namespace std;
using namespace kernel;
using namespace cv;

class FrameTestCase: public ::testing::Test
{
public:
    FrameTestCase()
            : gf(argsHDR)
    {
    }
protected:
    virtual void SetUp()
    {
        gf.getFrameFromFile(argsHDR.inputFiles[0]);
        frame = gf.getRawFrame();
    }

    // virtual void TearDown() {}

    GenericFrame gf;
    Mat frame;
};

TEST_F(FrameTestCase, FrameCreateNew)
{
    ASSERT_FALSE(frame.empty());
}

TEST(ExposureValueCase, TestShutterSpeedChange)
{
    double eps = 0.1;
    ExposureValue ev(0);
    cout << ev << endl;

    ev.setShutterSpeed(2);
    cout << ev << endl;
    EXPECT_NEAR(-1, ev.get(), eps);

    ev.setShutterSpeed(0.5);
    cout << ev << endl;
    EXPECT_NEAR(1, ev.get(), eps);

    ev.setShutterSpeed((float)1/8000);
    cout << ev << endl;
    EXPECT_NEAR(13, ev.get(), eps);

    ev.setShutterSpeed(60);
    cout << ev << endl;
    EXPECT_NEAR(-6, ev.get(), eps);
}

TEST(ExposureValueCase, TestApertureSpeedChange)
{
    double eps = 0.1;
    ExposureValue ev(0);
    cout << ev << endl;

    ev.setAperture(0.7071067811865475); // = pow(sqrt(2), -1)
    cout << ev << endl;
    EXPECT_NEAR(-1, ev.get(), eps);

    ev.setAperture(1.4142135623730951); // = pow(sqrt(2), 1)
    cout << ev << endl;
    EXPECT_NEAR(1, ev.get(), eps);

    ev.setAperture(90.50966799187816); // = pow(sqrt(2), 13)
    cout << ev << endl;
    EXPECT_NEAR(13, ev.get(), eps);

    ev.setAperture(0.12499999999999994); // = pow(sqrt(2), -6)
    cout << ev << endl;
    EXPECT_NEAR(-6, ev.get(), eps);
}

TEST(ExposureValueCase, TestISO)
{
    double eps = 0.1;
    ExposureValue ev(0);
    cout << ev << endl;

    ev.setISO(200);
    cout << ev << endl;
    EXPECT_NEAR(-1, ev.get(), eps);

    ev.setISO(50);
    cout << ev << endl;
    EXPECT_NEAR(1, ev.get(), eps);

    ev.setISO(100 * pow(2, -13));
    cout << ev << endl;
    EXPECT_NEAR(13, ev.get(), eps);

    ev.setISO(100 * pow(2, 6));
    cout << ev << endl;
    EXPECT_NEAR(-6, ev.get(), eps);
}

TEST(ExposureValueCase, TestAllSplet)
{
    double eps = 0.1;
    ExposureValue ev(0);
    cout << ev << endl;

    ev.setISO(100);
    ev.setAperture(2.0);
    EXPECT_NEAR(2, ev.get(), eps);
    ev.setShutterSpeed(15.0);
    EXPECT_NEAR(-2, ev.get(), eps);
    ev.setAperture(11.0);
    EXPECT_NEAR(3, ev.get(), eps);
    ev.setAperture(8.0);
    EXPECT_NEAR(2, ev.get(), eps);
    ev.setShutterSpeed(0.5);
    cout << ev << endl;
    EXPECT_NEAR(7, ev.get(), eps);
}

TEST(ExposureValueCase, TestPrograms)
{
    double eps = 1e-6;
    ExposureValue ev(0);
    cout << ev << endl;
    ev.setExpVal(1, ExposureValue::Speed);
    cout << ev << endl;
    EXPECT_NEAR(1, ev.get(), eps);
    ev.setExpVal(2, ExposureValue::Aperture);
    cout << ev << endl;
    EXPECT_NEAR(2, ev.get(), eps);
    ev.setExpVal(3, ExposureValue::ISO);
    cout << ev << endl;
    EXPECT_NEAR(3, ev.get(), eps);
    ev.setExpVal(4, ExposureValue::Auto);
    cout << ev << endl;
    EXPECT_NEAR(4, ev.get(), eps);
    ev.setExpVal(4, ExposureValue::ISO);
    cout << ev << endl;
    EXPECT_NEAR(4, ev.get(), eps);
    ev.setExpVal(3.5, ExposureValue::Aperture);
    cout << ev << endl;
    EXPECT_NEAR(3.5, ev.get(), eps);
    ev.setExpVal(3.3, ExposureValue::Speed);
    cout << ev << endl;
    EXPECT_NEAR(3.3, ev.get(), eps);
    ev.setExpVal(2, ExposureValue::Auto);
    cout << ev << endl;
    EXPECT_NEAR(2, ev.get(), eps);
    ev.setExpVal(-1.5, ExposureValue::ISO);
    cout << ev << endl;
    EXPECT_NEAR(-1.5, ev.get(), eps);
    ev.setExpVal(0, ExposureValue::Aperture);
    cout << ev << endl;
    EXPECT_NEAR(0, ev.get(), eps);

}


//  miscellaneous.cpp  ----------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes
//  Copyright 2009 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*
This code was extracted by Vicente J. Botet Escriba from Beman Dawes time2_demo.cpp which
was derived by Beman Dawes from Howard Hinnant's time2_demo prototype.
Many thanks to Howard for making his code available under the Boost license.
The original code was modified to conform to Boost conventions and to section
20.9 Time utilities [time] of the C++ committee's working paper N2798.
See http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2008/n2798.pdf.

time2_demo contained this comment:

    Much thanks to Andrei Alexandrescu,
                   Walter Brown,
                   Peter Dimov,
                   Jeff Garland,
                   Terry Golubiewski,
                   Daniel Krugler,
                   Anthony Williams.
*/

#include <boost/chrono/chrono.hpp>
#include <boost/type_traits.hpp>

#include <iostream>

// miscellaneous tests and demos:

#include <cassert>
#include <iostream>

using namespace boost::chrono;

void physics_function(duration<double> d)
{
    std::cout << "d = " << d.count() << '\n';
}

void drive_physics_function()
{
    physics_function(nanoseconds(3));
    physics_function(hours(3));
    physics_function(duration<double>(2./3));
    std::cout.precision(16);
    physics_function( hours(3) + nanoseconds(-3) );
}

void test_range()
{
    using namespace boost::chrono;
    hours h1 = hours(24 * ( 365 * 292 + 292/4));
    nanoseconds n1 = h1 + nanoseconds(1);
    nanoseconds delta = n1 - h1;
    std::cout << "292 years of hours = " << h1.count() << "hr\n";
    std::cout << "Add a nanosecond = " << n1.count() << "ns\n";
    std::cout << "Find the difference = " << delta.count() << "ns\n";
}

void test_extended_range()
{
    using namespace boost::chrono;
    hours h1 = hours(24 * ( 365 * 244000 + 244000/4));
    /*auto*/ microseconds u1 = h1 + microseconds(1);
    /*auto*/ microseconds delta = u1 - h1;
    std::cout << "244,000 years of hours = " << h1.count() << "hr\n";
    std::cout << "Add a microsecond = " << u1.count() << "us\n";
    std::cout << "Find the difference = " << delta.count() << "us\n";
}

template <class Rep, class Period>
void inspect_duration(boost::chrono::duration<Rep, Period> d, const std::string& name)
{
    typedef boost::chrono::duration<Rep, Period> Duration;
    std::cout << "********* " << name << " *********\n";
    std::cout << "The period of " << name << " is " << (double)Period::num/Period::den << " seconds.\n";
    std::cout << "The frequency of " << name << " is " << (double)Period::den/Period::num << " Hz.\n";
    std::cout << "The representation is ";
    if (boost::is_floating_point<Rep>::value)
    {
        std::cout << "floating point\n";
        std::cout << "The precision is the most significant ";
        std::cout << std::numeric_limits<Rep>::digits10 << " decimal digits.\n";
    }
    else if (boost::is_integral<Rep>::value)
    {
        std::cout << "integral\n";
        d = Duration(Rep(1));
        boost::chrono::duration<double> dsec = d;
        std::cout << "The precision is " << dsec.count() << " seconds.\n";
    }
    else
    {
        std::cout << "a class type\n";
        d = Duration(Rep(1));
        boost::chrono::duration<double> dsec = d;
        std::cout << "The precision is " << dsec.count() << " seconds.\n";
    }
    d = Duration((std::numeric_limits<Rep>::max)());
    using namespace boost::chrono;
    typedef duration<double, boost::ratio_multiply<boost::ratio<24*3652425,10000>, hours::period>::type> Years;
    Years years = d;
    std::cout << "The range is +/- " << years.count() << " years.\n";
    std::cout << "sizeof(" << name << ") = " << sizeof(d) << '\n';
}

void inspect_all()
{
    using namespace boost::chrono;
    std::cout.precision(6);
    inspect_duration(nanoseconds(), "nanoseconds");
    inspect_duration(microseconds(), "microseconds");
    inspect_duration(milliseconds(), "milliseconds");
    inspect_duration(seconds(), "seconds");
    inspect_duration(minutes(), "minutes");
    inspect_duration(hours(), "hours");
    inspect_duration(duration<double>(), "duration<double>");
}

void test_milliseconds()
{
    using namespace boost::chrono;
    milliseconds ms(250);
    ms += milliseconds(1);
    milliseconds ms2(150);
    milliseconds msdiff = ms - ms2;
    if (msdiff == milliseconds(101))
        std::cout << "success\n";
    else
        std::cout << "failure: " << msdiff.count() << '\n';
}

int main()
{
    using namespace boost;
    drive_physics_function();
    test_range();
    test_extended_range();
    inspect_all();
    test_milliseconds();
    inspect_duration(common_type<duration<double>, hours, microseconds>::type(),
                    "common_type<duration<double>, hours, microseconds>::type");
    duration<double, boost::milli> d = milliseconds(3) * 2.5;
    inspect_duration(milliseconds(3) * 2.5, "milliseconds(3) * 2.5");
    std::cout << d.count() << '\n';
//    milliseconds ms(3.5);  // doesn't compile
//    std::cout << "milliseconds ms(3.5) doesn't compile\n";
    return 0;
}


#include <iostream>
#include <fstream>

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

#include "glog/logging.h"

#include "dbscan_vp.h"

namespace po = boost::program_options;
using namespace clustering;

int main( int argc, char const* argv[] )
{
    std::string file_in;
    size_t k = 3;

    po::options_description option_desc( "kNN distance calculator" );

    option_desc.add_options()( "help", "Display help" )(
        "in,i", po::value< std::string >( &file_in )->required(), "Input file CSV format" )(
        "knn,k", po::value< size_t >( &k )->default_value( 3u ), "k-th neighbour" );

    po::variables_map options;

    po::store( po::command_line_parser( argc, argv ).options( option_desc ).run(), options );

    if ( options.empty() or options.count( "help" ) ) {
        std::cout << option_desc << std::endl;
        return 0;
    }

    po::notify( options );

    Dataset::Ptr dset = Dataset::create();
    LOG( INFO ) << "Loading dataset from " << file_in << " k = " << k;
    dset->load_csv( file_in );

    DBSCAN_VP::Ptr dbs = boost::make_shared< DBSCAN_VP >( dset );

    dbs->fit();

    LOG( INFO ) << "Fit time " << dbs->get_fit_time() << " seconds";

    const auto r = dbs->predict_eps( k );

    for ( size_t i = 0; i < r.size(); ++i ) {
        std::cout << i << "," << r[i] << std::endl;
    }

    return 0;
}


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

#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include <gmpxx.h>

#include "Random.h"

BOOST_AUTO_TEST_CASE(random_test_1)
{
    int num_bits = 256;
    mpz_class random_1 = Random::GenerateRandomNumberBits(num_bits);
    //gmp_printf("Random Number: %Zd\n", random_1.get_mpz_t());

    mpz_class random_2 = Random::GenerateRandomNumberBits(num_bits);
    //gmp_printf("Random_Number: %Zd\n", random_2.get_mpz_t());

    // verify that the splits, then rejoining to produce the original output
    BOOST_CHECK(random_1 != random_2);
    BOOST_CHECK_EQUAL(mpz_sizeinbase(random_1.get_mpz_t(), 2), num_bits);
    BOOST_CHECK_EQUAL(mpz_sizeinbase(random_2.get_mpz_t(), 2), num_bits);

}


/*
 * LightIntensityFilter.hpp
 *
 *  Created on: Sep 23, 2017
 *      Author: Peter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#pragma once

#include <Eigen/Core>
#include <string>

#include <filters/filter_base.hpp>
#include <grid_map_core/GridMap.hpp>

namespace grid_map {

/*!
 * Compute the diffuse lighting of a surface as new black and white color layer.
 */
class LightIntensityFilter : public filters::FilterBase<GridMap> {
 public:
  /*!
   * Constructor
   */
  LightIntensityFilter();

  /*!
   * Destructor.
   */
  ~LightIntensityFilter() override;

  /*!
   * Configures the filter from parameters on the Parameter Server
   */
  bool configure() override;

  /*!
   * Compute the diffuse lighting layer.
   * @param mapIn grid map containing the layers of the normal vectors.
   * @param mapOut grid map containing mapIn and the black and white lighting color layer.
   */
  bool update(const GridMap& mapIn, GridMap& mapOut) override;

 private:
  //! Input layers prefix.
  std::string inputLayersPrefix_;

  //! Output layer name.
  std::string outputLayer_;

  //! Light direction.
  Eigen::Vector3f lightDirection_;
};

}  // namespace grid_map