/* http://melpon.org/wandbox/permlink/paCC61fxm2muS5lc */

//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include "max_plus.hpp"
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <vector>

using namespace boost;

template <typename Graph, typename ParentMap> struct edge_writer {
  edge_writer(const Graph &g, const ParentMap &p) : m_g(g), m_parent(p) {}

  template <typename Edge>
  void operator()(std::ostream &out, const Edge &e) const {
    out << "[label=\"" << get(edge_weight, m_g, e) << "\"";
    typename graph_traits<Graph>::vertex_descriptor u = source(e, m_g),
                                                    v = target(e, m_g);
    if (m_parent[v] == u)
      out << ", color=\"black\"";
    else
      out << ", color=\"grey\"";
    out << "]";
  }
  const Graph &m_g;
  ParentMap m_parent;
};
template <typename Graph, typename Parent>
edge_writer<Graph, Parent> make_edge_writer(const Graph &g, const Parent &p) {
  return edge_writer<Graph, Parent>(g, p);
}

struct EdgeProperties {
  fun::maxPlus<int> weight;
};

int main() {
  enum { u, v, x, y, z, N };
  char name[] = {'u', 'v', 'x', 'y', 'z'};
  typedef std::pair<int, int> E;
  const int n_edges = 10;
  E edge_array[] = {E(u, y), E(u, x), E(u, v), E(v, u), E(x, y),
                    E(x, v), E(y, v), E(y, z), E(z, u), E(z, x)};
  fun::maxPlus<int> weight[n_edges] = {-4, 8, 5, -2, 9, -3, 7, 2, 6, 7};

  typedef adjacency_list<vecS, vecS, directedS, no_property, EdgeProperties>
      Graph;
  Graph g(edge_array, edge_array + n_edges, N);

  graph_traits<Graph>::edge_iterator ei, ei_end;
  property_map<Graph, fun::maxPlus<int> EdgeProperties::*>::type weight_pmap =
      get(&EdgeProperties::weight, g);
  int i = 0;
  for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei, ++i)
    weight_pmap[*ei] = weight[i];

  std::vector<fun::maxPlus<int>> distance(N,
                                          (std::numeric_limits<short>::max)());
  std::vector<std::size_t> parent(N);
  for (i = 0; i < N; ++i)
    parent[i] = i;
  distance[z] = 0;

  bool r = bellman_ford_shortest_paths(g, int(N),
                                       weight_map(weight_pmap)
                                           .distance_map(&distance[0])
                                           .predecessor_map(&parent[0]));

  if (r)
    for (i = 0; i < N; ++i)
      std::cout << name[i] << ": " << std::setw(3) << distance[i] << " "
                << name[parent[i]] << std::endl;
  else
    std::cout << "negative cycle" << std::endl;
  std::cout << '\n';

  // std::ofstream dot_file("figs/bellman-eg.dot");
  auto &dot_file = std::cout;

  dot_file << "digraph D {\n"
           << "  rankdir=LR\n"
           << "  size=\"5,3\"\n"
           << "  ratio=\"fill\"\n"
           << "  edge[style=\"bold\"]\n"
           << "  node[shape=\"circle\"]\n";

  {
    for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
      graph_traits<Graph>::edge_descriptor e = *ei;
      graph_traits<Graph>::vertex_descriptor u = source(e, g), v = target(e, g);
      // VC++ doesn't like the 3-argument get function, so here
      // we workaround by using 2-nested get()'s.
      dot_file << "  " << name[u] << " -> " << name[v] << "[label=\""
               << get(get(&EdgeProperties::weight, g), e) << "\"";
      if (parent[v] == u)
        dot_file << ", color=\"black\"";
      else
        dot_file << ", color=\"grey\"";
      dot_file << "]\n";
    }
  }
  dot_file << "}\n";
  return EXIT_SUCCESS;
}

//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <vector>

//#include "max_plus.hpp"

using namespace boost;

template <typename Graph, typename ParentMap> struct edge_writer {
  edge_writer(const Graph &g, const ParentMap &p) : m_g(g), m_parent(p) {}

  template <typename Edge>
  void operator()(std::ostream &out, const Edge &e) const {
    out << "[label=\"" << get(edge_weight, m_g, e) << "\"";
    typename graph_traits<Graph>::vertex_descriptor u = source(e, m_g),
                                                    v = target(e, m_g);
    if (m_parent[v] == u)
      out << ", color=\"black\"";
    else
      out << ", color=\"grey\"";
    out << "]";
  }
  const Graph &m_g;
  ParentMap m_parent;
};
template <typename Graph, typename Parent>
edge_writer<Graph, Parent> make_edge_writer(const Graph &g, const Parent &p) {
  return edge_writer<Graph, Parent>(g, p);
}

struct EdgeProperties {
  int weight;
};

int main() {
  enum { u, v, x, y, z, N };
  char name[] = {'u', 'v', 'x', 'y', 'z'};
  typedef std::pair<int, int> E;
  const int n_edges = 10;
  E edge_array[] = {E(u, y), E(u, x), E(u, v), E(v, u), E(x, y),
                    E(x, v), E(y, v), E(y, z), E(z, u), E(z, x)};
  int weight[n_edges] = {-4, 8, 5, -2, 9, -3, 7, 2, 6, 7};

  typedef adjacency_list<vecS, vecS, directedS, no_property, EdgeProperties>
      Graph;
  Graph g(edge_array, edge_array + n_edges, N);

  graph_traits<Graph>::edge_iterator ei, ei_end;
  property_map<Graph, int EdgeProperties::*>::type weight_pmap =
      get(&EdgeProperties::weight, g);
  int i = 0;
  for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei, ++i)
    weight_pmap[*ei] = weight[i];

  std::vector<int> distance(N, (std::numeric_limits<short>::max)());
  std::vector<std::size_t> parent(N);
  for (i = 0; i < N; ++i)
    parent[i] = i;
  distance[z] = 0;

  bool r = bellman_ford_shortest_paths(g, int(N),
                                       weight_map(weight_pmap)
                                           .distance_map(&distance[0])
                                           .predecessor_map(&parent[0]));

  if (r)
    for (i = 0; i < N; ++i)
      std::cout << name[i] << ": " << std::setw(3) << distance[i] << " "
                << name[parent[i]] << std::endl;
  else
    std::cout << "negative cycle" << std::endl;
  std::cout << '\n';

  // std::ofstream dot_file("figs/bellman-eg.dot");
  auto &dot_file = std::cout;

  dot_file << "digraph D {\n"
           << "  rankdir=LR\n"
           << "  size=\"5,3\"\n"
           << "  ratio=\"fill\"\n"
           << "  edge[style=\"bold\"]\n"
           << "  node[shape=\"circle\"]\n";

  {
    for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
      graph_traits<Graph>::edge_descriptor e = *ei;
      graph_traits<Graph>::vertex_descriptor u = source(e, g), v = target(e, g);
      // VC++ doesn't like the 3-argument get function, so here
      // we workaround by using 2-nested get()'s.
      dot_file << "  " << name[u] << " -> " << name[v] << "[label=\""
               << get(get(&EdgeProperties::weight, g), e) << "\"";
      if (parent[v] == u)
        dot_file << ", color=\"black\"";
      else
        dot_file << ", color=\"grey\"";
      dot_file << "]\n";
    }
  }
  dot_file << "}\n";
  return EXIT_SUCCESS;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2020 Digvijay Janartha, Hamirpur, India.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <geometry_test_common.hpp>

#include <boost/core/ignore_unused.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/algorithms/append.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/concepts/ring_concept.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/io/dsv/write.hpp>

#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

#ifdef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
#include <initializer_list>
#endif//BOOST_NO_CXX11_HDR_INITIALIZER_LIST

template <typename P>
bg::model::ring<P> create_ring()
{
    bg::model::ring<P> r1;
    P p1;
    P p2;
    P p3;
    P p4;
    bg::assign_values(p1, 2, 2);
    bg::assign_values(p2, 2, 0);
    bg::assign_values(p3, 0, 0);
    bg::assign_values(p4, 0, 2);

    bg::append(r1, p1);
    bg::append(r1, p2);
    bg::append(r1, p3);
    bg::append(r1, p4);
    bg::append(r1, p1);
    return r1;
}

template <typename P, typename T>
void check_point(P& to_check, T x, T y)
{
    BOOST_CHECK_EQUAL(bg::get<0>(to_check), x);
    BOOST_CHECK_EQUAL(bg::get<1>(to_check), y);
}

template <typename R, typename P>
void check_ring(R& to_check, P p1, P p2, P p3, P p4)
{
    check_point(to_check[0], bg::get<0>(p1), bg::get<1>(p1));
    check_point(to_check[1], bg::get<0>(p2), bg::get<1>(p2));
    check_point(to_check[2], bg::get<0>(p3), bg::get<1>(p3));
    check_point(to_check[3], bg::get<0>(p4), bg::get<1>(p4));
    check_point(to_check[4], bg::get<0>(p1), bg::get<1>(p1));
}

template <typename P>
void test_default_constructor()
{
    bg::model::ring<P> r1(create_ring<P>());
    check_ring(r1, P(2, 2), P(2, 0), P(0, 0), P(0, 2));
}

template <typename P>
void test_copy_constructor()
{
    bg::model::ring<P> r1 = create_ring<P>();
    check_ring(r1, P(2, 2), P(2, 0), P(0, 0), P(0, 2));
}

template <typename P>
void test_copy_assignment()
{
    bg::model::ring<P> r1(create_ring<P>()), r2;
    r2 = r1;
    check_ring(r2, P(2, 2), P(2, 0), P(0, 0), P(0, 2));
}

template <typename P>
void test_concept()
{
    typedef bg::model::ring<P> R;

    BOOST_CONCEPT_ASSERT( (bg::concepts::ConstRing<R>) );
    BOOST_CONCEPT_ASSERT( (bg::concepts::Ring<R>) );

    typedef typename bg::coordinate_type<R>::type T;
    typedef typename bg::point_type<R>::type PR;
    boost::ignore_unused<T, PR>();
}

template <typename P>
void test_all()
{
    test_default_constructor<P>();
    test_copy_constructor<P>();
    test_copy_assignment<P>();
    test_concept<P>();
}

template <typename P>
void test_custom_ring(bg::model::ring<P> IL)
{
    bg::model::ring<P> r1(IL);
    std::ostringstream out;
    out << bg::dsv(r1);
    BOOST_CHECK_EQUAL(out.str(), "((3, 3), (3, 0), (0, 0), (0, 3), (3, 3))");
}

template <typename P>
void test_custom()
{
#ifdef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
    std::initializer_list<P> IL = {P(3, 3), P(3, 0), P(0, 0), P(0, 3), P(3, 3)};
    test_custom_ring<P>(IL);
#endif//BOOST_NO_CXX11_HDR_INITIALIZER_LIST
}

template <typename CS>
void test_cs()
{
    test_all<bg::model::point<int, 2, CS> >();
    test_all<bg::model::point<float, 2, CS> >();
    test_all<bg::model::point<double, 2, CS> >();

    test_custom<bg::model::point<double, 2, CS> >();
}

int test_main(int, char* [])
{
    test_cs<bg::cs::cartesian>();
    test_cs<bg::cs::spherical<bg::degree> >();
    test_cs<bg::cs::spherical_equatorial<bg::degree> >();
    test_cs<bg::cs::geographic<bg::degree> >();

    test_custom<bg::model::d2::point_xy<double> >();

    return 0;
}


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   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.
*/

// Critical temperature of square lattice Ising model

// reference: L. Onsager, Phys. Rev. 65, 117 (1944)

#pragma once

#include <cmath>
#include <stdexcept>
#include <boost/math/differentiation/autodiff.hpp>
#include <standards/newton.hpp>

namespace {

template<typename T>
struct func {
  func(T Jx, T Jy) : Jx_(Jx), Jy_(Jy) {}
  auto operator()(T beta) const -> decltype(boost::math::differentiation::make_fvar<T, 1>(beta)) {
    using std::sinh;
    auto beta_fvar = boost::math::differentiation::make_fvar<T, 1>(beta);
    return sinh(2 * Jx_ * beta_fvar) * sinh(2 * Jy_ * beta_fvar) - 1;
  }
  T Jx_, Jy_;
};

}

namespace ising {
namespace tc {

template<typename T>
inline T square(T Jx, T Jy) {
  Jx = abs(Jx);
  Jy = abs(Jy);
  if (Jx * Jy <= 0) throw(std::invalid_argument("Jx * Jy should be non-zero"));
  auto result = standards::newton_1d(func<T>(Jx, Jy), 1 / (2 * (Jx + Jy)));
  if (!result.second) throw(std::runtime_error("convergence error"));
  return 1 / result.first;
}

} // end namespace tc
} // end namespace ising


#include <cstdlib>
#include <iostream>
#include <cmath>
#include <ctime>
#include <string>
#include <vector>
#include <fstream>
#include <algorithm>
#include <Eigen/Dense>

using Eigen::MatrixXd;
using Eigen::VectorXd;
using namespace std;

const int MATCH_MIN = 0;
const int MATCH_MAX = 1;

/*
 * readMatrix
 * read a file into a matrix object
 * file must have matrix dimension as first line
 * matrix must be square
 */
void readMatrix(const char* filename, MatrixXd& m) {
  ifstream fin(filename);

  if (!fin) {
    cout << "Cannot open file." << endl;
    return;
  }

  // read in matrix dimensions and
  // create a matrix of that size
  int n_rows, n_cols;
  fin >> n_rows >> n_cols;
  MatrixXd temp(n_rows, n_cols);

  // read elements into the temporary matrix
  for (int i=0; i<n_rows; i++) {
    for (int j=0; j<n_cols; j++) {
      fin >> temp(i,j);
    }
  }
  fin.close();

  // if the dimensions are equal (square matrix), we're done
  // else, have to figure out larger dimension and pad with matrix max
  if (n_rows == n_cols) {
    m = temp;
  }
  else {
    float max_elem = temp.maxCoeff(); // find the max element
    float dim = max(n_rows, n_cols); // find the dimension for the new, square matrix
    m.resize(dim,dim);
    // fill the matrix with the elements from temp and pad with max element
    for (int i=0; i < dim; i++) {
      for (int j=0; j < dim; j++) {
        if (i >= n_rows || j >= n_cols) m(i,j) = max_elem;
        else m(i,j) = temp(i,j);
      }
    }
  }
}

/*
 * reduce
 * reduces matrix based on row and column minimums
 */
void reduce(MatrixXd& m) {
  // subtract row minimum from each row
  for (int i=0; i<m.rows(); i++) {
    float minElement = m.row(i).minCoeff();
    VectorXd rMinusMin(m.rows());
    rMinusMin.fill(-minElement);
    m.row(i) += rMinusMin;
  }
}

/*
 * hasMark
 * if there is a starred/primed zero in the given row/col, returns it's index
 * else, returns -1
 */
int hasMark(VectorXd& v) {
  for (int i=0; i<v.size(); i++) {
    if (v(i)) {
      return i;
    }
  }
  return -1;
}

/*
 * swapStarsAndPrimes
 * Swap stars and primes based on step 5 of Hungarian algorithm
 * Z0 is uncovered primed zero we've found
 * Z1 is the stared zero in the column of Z0 (if any)
 * Z2 is the primed zero in the row of Z1 (will always be one)
 * ...continue series until we reach a primed zero with no starred zero in its column
 * Unstar each starred zero, star each primed zero, erase all primes and uncover every line in the matrix
 */
void swapStarsAndPrimes(int i, int j, MatrixXd& stars, MatrixXd& primes) {
  int primeRow = i;
  int primeCol = j;

  bool done = false;
  while (!done) {
    // find row index of row that has a 0* in the same col as the current 0'
    VectorXd col = stars.col(primeCol);
    int starInPrimeColRow = hasMark(col);

    if (starInPrimeColRow < 0) {
      // star the prime we're looking at
      primes(primeRow, primeCol) = 0;
      stars(primeRow, primeCol) = 1;
      done = true;
    }
    else {
      // find which col has a 0' in the same row as z1
      VectorXd row = primes.row(starInPrimeColRow);
      int primeInStarRowCol = hasMark(row);

      // star first primed zero
      primes(primeRow, primeCol) = 0;
      stars(primeRow, primeCol) = 1;
      //primes(starInPrimeColRow, primeInStarRowCol) = 0;
      //stars(starInPrimeColRow, primeInStarRowCol) = 1;

      // unstar starred zero
      stars(starInPrimeColRow, primeCol) = 0;

      // set index of last prime, will check it's column for 0*s next
      primeRow = starInPrimeColRow;
      primeCol = primeInStarRowCol;
    }
  }
  // clear primes
  primes.fill(0);
}

/*
 * findMatching
 * implementation of the Hungarian matching algorithm
 * referenced from: http://csclab.murraystate.edu/bob.pilgrim/445/munkres.html
 */
void findMatching(MatrixXd& m, MatrixXd& result, int type) {
  MatrixXd n = m; // make a copy of m for reducing
  int dim = n.rows(); // dimension of matrix, used for checking if we've reduced
                      // the matrix enough yet

  MatrixXd stars(m.rows(), m.cols()); // matrix for storing our "starred" 0s (0*)
  stars.fill(0);
  MatrixXd primes(m.rows(), m.cols()); // matrix for storing our "primed" 0s (0')
  primes.fill(0);
  VectorXd rowCover(m.rows()); // keep track of which rows are "covered"
  rowCover.fill(0);
  VectorXd colCover(m.cols()); // keep track of which columns are "covered"
  colCover.fill(0);

  // to do maximization rather than minimization, we have to
  // transform the matrix by subtracting every value from the maximum
  if (type == MATCH_MAX) {
    float max = n.maxCoeff();
    MatrixXd maxMat(n.rows(), n.cols());
    maxMat.fill(max);
    n = maxMat - n;
  }

  // Step 1
  // Reduce matrix
  reduce(n);

  // Step 2
  // Find a zero in the matrix. If there is no starred zero in
  // its row or column, star Z. Repeat for each element in the matrix.
  for (int i=0; i<n.rows(); i++) {
    for (int j=0; j<n.cols(); j++) {
      if (n(i,j) == 0 && !rowCover(i) && !colCover(j)) {
        stars(i,j) = 1;
        rowCover(i) = 1;
        colCover(j) = 1;
      }
    }
  }
  // covers need to be cleared for following steps
  rowCover.fill(0);
  colCover.fill(0);

  while (true) {
    // Step 3
    // Cover all columns that have a starred zero
    // If the number of columns with starred zeroes equals the matrix
    // dimensions, we are done! Otherwise, move on to step 4.
    step3:
    for (int j=0; j<n.cols(); j++) {
      VectorXd col = stars.col(j);
      if (hasMark(col) >= 0) {
        colCover(j) = 1;
      }
    }
    if (colCover.sum() == dim) {
      result = stars;
      return;
    }

    // Step 4
    // Find a non-covered zero and prime it
    step4:
    for (int i=0; i<n.rows(); i++) {
      for (int j=0; j<n.cols(); j++) {
        if (n(i,j) == 0 && !rowCover(i) && !colCover(j)) {
          primes(i,j) = 1;
          // if no starred zero in the row...
          VectorXd row = stars.row(i);
          if (hasMark(row) < 0) {
            // Step 5
            // swap stars and primes
            swapStarsAndPrimes(i, j, stars, primes);

            // clear lines
            rowCover.fill(0);
            colCover.fill(0);

            goto step3;
          }
          else {
            // cover row
            rowCover(i) = 1;

            // uncover column of the starred zero in the same row
            int col = hasMark(row);
            colCover(col) = 0;
          }
        }
      }
    }

    // Step 6
    // Should now be no more uncovered zeroes
    // Get the minimum uncovered element
    float min = 1000000;
    for (int i=0; i<n.rows(); i++) {
      for (int j=0; j<n.cols(); j++) {
        if (!rowCover(i) && !colCover(j) && n(i,j) < min) {
          min = n(i,j);
        }
      }
    }

    // Subtract minimum from uncovered elements, add it to elements covered twice
    for (int i=0; i<n.rows(); i++) {
      for (int j=0; j<n.cols(); j++) {
        if (!rowCover(i) && !colCover(j)) {
          n(i,j) -= min;
        }
        else if (rowCover(i) && colCover(j)) {
          n(i,j) += min;
        }
      }
    }

    goto step4;
  }
}

/*
 * main()
 */
int main(int argc, char *argv[])
{
  if (argc != 2) {
    cout << "Usage: ./a.out [matrixfile]" << endl;
    return 0;
  }

  // times
	clock_t start, end;
  float elapsedTime;

  // random seed
  srand((unsigned)time(0));

	// start time
  start = clock();

  // read in the matrix file
  MatrixXd m(1,1);
  readMatrix(argv[1], m);

  // create an empty matrix to put the result in
  MatrixXd result(m.rows(), m.cols());
  result.fill(0);

  // run the Hungarian (Munkres) algorithm to find the maximal matching
  findMatching(m, result, MATCH_MIN);

  MatrixXd mask = m.cwiseProduct(result);
  int sum = mask.sum();

  cout << "ORIGINAL MATRIX" << endl;
  cout << m << endl;
  cout << endl;
  cout << "ASSIGNMENT RESULT" << endl;
  cout << result << endl;
  cout << endl;
  cout << "FINAL SUM" << endl;
  cout << sum << endl;

  // stop time
  end = clock();
	elapsedTime = (float)(end - start) / CLOCKS_PER_SEC;
  cout << "Completed in: " << elapsedTime << " s" << endl;

  return 0;
}


#include <algorithm>
#include <iostream>
#include <list>
#include <numeric>
#include <random>
#include <vector>
#include "cnn/metric-util.h"
#include <initializer_list>
#include <Eigen/LU>

using namespace std;
namespace cnn { namespace metric {

    cnn::real cosine_similarity(const std::vector<cnn::real> &s1, const std::vector<cnn::real> &s2)
    {
        cnn::real flt = 0.0;
        cnn::real flt1 = 0.0, flt2 = 0.0;

        for (int k = 0; k < s1.size(); k++)
        {
            flt += s1[k] * s2[k];
            flt1 += s1[k] * s1[k];
            flt2 += s2[k] * s2[k];
        }

        if (flt1 == 0.0) flt1 = FLT_EPSILON;
        if (flt2 == 0.0) flt2 = FLT_EPSILON;
        flt1 = sqrt(flt1);
        flt2 = sqrt(flt2);

        cnn::real val = flt / (flt1 * flt2);

        return val;
    }

    int levenshtein_distance(const vector<std::string> &s1, const vector<std::string> &s2)
    {
        // To change the type this function manipulates and returns, change
        // the return type and the types of the two variables below.
        int s1len = s1.size();
        int s2len = s2.size();

        auto column_start = (decltype(s1len))1;

        auto column = new decltype(s1len)[s1len + 1];
        std::iota(column + column_start, column + s1len + 1, column_start);

        for (auto x = column_start; x <= s2len; x++) {
            column[0] = x;
            auto last_diagonal = x - column_start;
            for (auto y = column_start; y <= s1len; y++) {
                auto old_diagonal = column[y];
                auto possibilities = {
                    column[y] + 1,
                    column[y - 1] + 1,
                    last_diagonal + (s1[y - 1] == s2[x - 1] ? 0 : 1)
                };
                column[y] = std::min(possibilities);
                last_diagonal = old_diagonal;
            }
        }
        auto result = column[s1len];
        delete[] column;
        return result;
    }
} }


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <iostream>
#include <memory>

#include <BayesFilters/AdditiveStateModel.h>
#include <BayesFilters/GaussianLikelihood.h>
#include <BayesFilters/GPFPrediction.h>
#include <BayesFilters/GPFCorrection.h>
#include <BayesFilters/InitSurveillanceAreaGrid.h>
#include <BayesFilters/Resampling.h>
#include <BayesFilters/SimulatedLinearSensor.h>
#include <BayesFilters/SimulatedStateModel.h>
#include <BayesFilters/SIS.h>
#include <BayesFilters/UKFPrediction.h>
#include <BayesFilters/UKFCorrection.h>
#include <BayesFilters/utils.h>
#include <BayesFilters/WhiteNoiseAcceleration.h>

#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

class UPFSimulation : public SIS
{
public:
    UPFSimulation
    (
        std::size_t num_particle,
        std::size_t state_size,
        std::size_t simulation_steps,
        /* Initial covariance of the Gaussian belief associated to each particle. */
        Ref<MatrixXd> initial_covariance,
        std::unique_ptr<ParticleSetInitialization> initialization,
        std::unique_ptr<PFPrediction> prediction,
        std::unique_ptr<PFCorrection> correction,
        std::unique_ptr<Resampling> resampling
    ) noexcept :
        SIS(num_particle, state_size, std::move(initialization), std::move(prediction), std::move(correction), std::move(resampling)),
        simulation_steps_(simulation_steps),
        initial_covariance_(initial_covariance)
    { }

protected:
    bool runCondition() override
    {
        if (getFilteringStep() < simulation_steps_)
            return true;
        else
            return false;
    }

    std::vector<std::string> log_filenames(const std::string& prefix_path, const std::string& prefix_name) override
    {
        std::vector<std::string> sis_filenames = SIS::log_filenames(prefix_path, prefix_name);

        /* Add file names for logging of the conditional expected value. */
        sis_filenames.push_back(prefix_path + "/" + prefix_name + "_mean");

        return  sis_filenames;
    }

    VectorXd mean_extraction(const ParticleSet& particles) const
    {
        /* Extract the conditional expected value of the filtered state
           given all the measurements up to the current time step. */
        return particles.state() * particles.weight().array().exp().matrix();
    }

    bool initialization() override
    {
        if (!SIS::initialization())
            return false;

        /* Initialize initial mean and covariance for each particle. */
        for (std::size_t i = 0; i < pred_particle_.components; i++)
        {
            /* Set mean equal to the particle position. */
            pred_particle_.mean(i) = pred_particle_.state(i);

            /* Set the covariance obtained within the ctor. */
            pred_particle_.covariance(i) = initial_covariance_;
        }

        return true;
    }

    void log() override
    {
        VectorXd mean = mean_extraction(cor_particle_);

        logger(pred_particle_.state().transpose(), pred_particle_.weight().transpose(),
               cor_particle_.state().transpose(), cor_particle_.weight().transpose(),
               mean.transpose());
    }

private:
    std::size_t simulation_steps_;

    Eigen::MatrixXd initial_covariance_;
};

int main()
{
    std::cout << "Running an unscented particle filter on a simulated target." << std::endl;
    std::cout << "Data is logged in the test folder with prefix testUPF." << std::endl;

    /* A set of parameters needed to run an unscented particle filter in a simulated environment. */
    double surv_x = 1000.0;
    double surv_y = 1000.0;
    std::size_t num_particle_x = 100;
    std::size_t num_particle_y = 100;
    std::size_t num_particle = num_particle_x * num_particle_y;
    Vector4d initial_state(10.0f, 0.0f, 10.0f, 0.0f);
    std::size_t simulation_time = 100;
    std::size_t state_size = 4;

    /* Unscented transform parameters.*/
    double alpha = 1.0;
    double beta = 2.0;
    double kappa = 0.0;

    /* Step 1 - Initialization */

    Matrix4d initial_covariance;
    initial_covariance << pow(0.05, 2), 0,            0,            0,
                          0,            pow(0.05, 2), 0,            0,
                          0,            0,            pow(0.01, 2), 0,
                          0,            0,            0,            pow(0.01, 2);

    /* Initialize particle initialization class. */
    std::unique_ptr<ParticleSetInitialization> grid_initialization = utils::make_unique<InitSurveillanceAreaGrid>(surv_x, surv_y, num_particle_x, num_particle_y);

    /* Step 2 - Prediction */

    /* Step 2.1 - Define the state model. */

    /* Initialize a white noise acceleration state model. */
    double T = 1.0f;
    double tilde_q = 10.0f;

    std::unique_ptr<AdditiveStateModel> wna = utils::make_unique<WhiteNoiseAcceleration>(T, tilde_q);

    /* Step 2.2 - Define the prediction step */

    /* Initialize the kalman particle filter prediction step that wraps a Gaussian prediction step,
       in this case an unscented kalman filter prediction step. */
    std::unique_ptr<GaussianPrediction> upf_prediction = utils::make_unique<UKFPrediction>(std::move(wna), state_size, alpha, beta, kappa);
    std::unique_ptr<PFPrediction> gpf_prediction = utils::make_unique<GPFPrediction>(std::move(upf_prediction));

    /* Step 3 - Correction */

    /* Step 3.1 - Define where the measurement are originated from (simulated in this case). */

    /* Initialize simulated target model with a white noise acceleration. */
    std::unique_ptr<StateModel> target_model = utils::make_unique<WhiteNoiseAcceleration>(T, tilde_q);
    std::unique_ptr<SimulatedStateModel> simulated_state_model = utils::make_unique<SimulatedStateModel>(std::move(target_model), initial_state, simulation_time);
    simulated_state_model->enable_log(".", "testUPF");

    /* Initialize a measurement model (a linear sensor reading x and y coordinates). */
    std::unique_ptr<AdditiveMeasurementModel> simulated_linear_sensor = utils::make_unique<SimulatedLinearSensor>(std::move(simulated_state_model));
    simulated_linear_sensor->enable_log(".", "testUPF");

    /* Step 3.2 - Define the likelihood model. */

    /* Initialize an exponential likelihood as measurement likelihood. */
    std::unique_ptr<LikelihoodModel> exp_likelihood = utils::make_unique<GaussianLikelihood>();

    /* Step 3.3 - Define the correction step. */

    /* An additional state model is required to make the transitionProbability of the state model available
       to the particle filter correction step. */
    std::unique_ptr<StateModel> transition_probability_model = utils::make_unique<WhiteNoiseAcceleration>(T, tilde_q);

    /* Initialize the particle filter correction step that wraps a Guassian correction step,
       in this case an unscented kalman filter correction step. */
    std::unique_ptr<GaussianCorrection> upf_correction = utils::make_unique<UKFCorrection>(std::move(simulated_linear_sensor), state_size, alpha, beta, kappa);
    std::unique_ptr<PFCorrection> gpf_correction = utils::make_unique<GPFCorrection>(std::move(upf_correction), std::move(exp_likelihood), std::move(transition_probability_model));

    /* Step 4 - Resampling */

    /* Initialize a resampling algorithm. */
    std::unique_ptr<Resampling> resampling = utils::make_unique<Resampling>();

    /* Step 5 - Assemble the particle filter. */
    std::cout << "Constructing unscented particle filter..." << std::flush;
    UPFSimulation upf(num_particle, state_size, simulation_time, initial_covariance, std::move(grid_initialization), std::move(gpf_prediction), std::move(gpf_correction), std::move(resampling));
    upf.enable_log(".", "testUPF");
    std::cout << "done!" << std::endl;

    /* Step 6 - Prepare the filter to be run */
    std::cout << "Booting unscented particle filter..." << std::flush;
    upf.boot();
    std::cout << "completed!" << std::endl;

    /* Step 7 - Run the filter and wait until it is closed */
    /* Note that since this is a simulation, the filter will end upon simulation termination */
    std::cout << "Running unscented particle filter..." << std::flush;
    upf.run();
    std::cout << "waiting..." << std::flush;

    if (!upf.wait())
        return EXIT_FAILURE;

    std::cout << "completed!" << std::endl;

    return EXIT_SUCCESS;
}


#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;

    double           array[][3]= {{1., 2., 3.}, {4., 5., 6.}, {7., 8., 9.}};
    dense2D<double>  A(array), A2, A3;

    // Creating a permutation matrix from a vector (or an array respectively)
    int indices[]= {1, 2, 0};
    mat::traits::permutation<>::type P= mat::permutation(indices);
    std::cout << "\nP =\n" << P;

    // Permutating rows
    A2= P * A;
    std::cout << "\nP * A =\n" << A2;

    // Permutating columns
    A3= A2 * trans(P);
    std::cout << "\nA2 * trans(P) =\n" << A3;

    dense_vector<double> v(array[2]), w(P * v);
    std::cout << "\nP * v =\n" << w << "\n";

    return 0;
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/collision/gjk/gjk_signed_distance_to_edge_face_voronoi_plane.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>

using namespace OpenTissue;

BOOST_AUTO_TEST_SUITE(opentissue_collision_gjk_signed_distance_to_edge_face_vp);

BOOST_AUTO_TEST_CASE(case_by_case_test)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::real_type                            real_type;

  vector3_type a = vector3_type(0.0, 0.0, 0.0);
  vector3_type b = vector3_type(1.0, 0.0, 0.0);;
  vector3_type c = vector3_type(0.0, 1.0, 0.0);;

  // Front side of AB voronoi plane
  {
    vector3_type p = vector3_type(-0.5, -1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, b, c);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, a, c);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }
  // Back side of AB voronoi plane
  {
    vector3_type p = vector3_type(-0.5, 1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, b, c);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, a, c);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }
  // In AB voronoi plane
  {
    vector3_type p = vector3_type(-0.5, 0.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, b, c);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, a, c);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }
  // Front side of AC voronoi plane
  {
    vector3_type p = vector3_type(-1.0, 0.5,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, c, b);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, a, b);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }
  // Back side of AC voronoi plane
  {
    vector3_type p = vector3_type( 1.0, 0.5,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, c, b);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, a, b);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }
  // In AC voronoi plane
  {
    vector3_type p = vector3_type( 0.0, 0.5,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, c, b);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, a, b);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }
  // Front side of BC voronoi plane
  {
    vector3_type p = vector3_type( 1.0, 1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, c, a);
    BOOST_CHECK_CLOSE( sign_p, 0.70710678118654752440084436210485, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, b, a);
    BOOST_CHECK_CLOSE( sign_p, 0.70710678118654752440084436210485, 0.01 );
  }
  // Back side of BC voronoi plane
  {
    vector3_type p = vector3_type( 0.0, 0.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, c, a);
    BOOST_CHECK_CLOSE( sign_p, -0.70710678118654752440084436210485, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, b, a);
    BOOST_CHECK_CLOSE( sign_p, -0.70710678118654752440084436210485, 0.01 );
  }
  // In BC voronoi plane
  {
    vector3_type p = vector3_type( 0.5, 0.5,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, c, a);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, b, a);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }

  // We just used a point above the face plane, next we will try using a test point lying in the face plane

  // Front side of AB voronoi plane
  {
    vector3_type p = vector3_type(-0.5, -1.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, b, c);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, a, c);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }
  // Back side of AB voronoi plane
  {
    vector3_type p = vector3_type(-0.5, 1.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, b, c);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, a, c);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }
  // In AB voronoi plane
  {
    vector3_type p = vector3_type(-0.5, 0.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, b, c);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, a, c);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }
  // Front side of AC voronoi plane
  {
    vector3_type p = vector3_type(-1.0, 0.5,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, c, b);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, a, b);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }
  // Back side of AC voronoi plane
  {
    vector3_type p = vector3_type( 1.0, 0.5,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, c, b);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, a, b);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }
  // In AC voronoi plane
  {
    vector3_type p = vector3_type( 0.0, 0.5,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, c, b);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, a, b);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }
  // Front side of BC voronoi plane
  {
    vector3_type p = vector3_type( 1.0, 1.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, c, a);
    BOOST_CHECK_CLOSE( sign_p, 0.70710678118654752440084436210485, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, b, a);
    BOOST_CHECK_CLOSE( sign_p, 0.70710678118654752440084436210485, 0.01 );
  }
  // Back side of BC voronoi plane
  {
    vector3_type p = vector3_type( 0.0, 0.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, c, a);
    BOOST_CHECK_CLOSE( sign_p, -0.70710678118654752440084436210485, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, b, a);
    BOOST_CHECK_CLOSE( sign_p, -0.70710678118654752440084436210485, 0.01 );
  }
  // In BC voronoi plane
  {
    vector3_type p = vector3_type( 0.5, 0.5,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, c, a);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, b, a);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }

  // Finally we will use a test point lying below the face-plane

  // Front side of AB voronoi plane
  {
    vector3_type p = vector3_type(-0.5, -1.0,  -1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, b, c);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, a, c);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }
  // Back side of AB voronoi plane
  {
    vector3_type p = vector3_type(-0.5, 1.0,  -1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, b, c);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, a, c);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }
  // In AB voronoi plane
  {
    vector3_type p = vector3_type(-0.5, 0.0,  -1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, b, c);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, a, c);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }
  // Front side of AC voronoi plane
  {
    vector3_type p = vector3_type(-1.0, 0.5,  -1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, c, b);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, a, b);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }
  // Back side of AC voronoi plane
  {
    vector3_type p = vector3_type( 1.0, 0.5,  -1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, c, b);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, a, b);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }
  // In AC voronoi plane
  {
    vector3_type p = vector3_type( 0.0, 0.5,  -1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, a, c, b);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, a, b);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }
  // Front side of BC voronoi plane
  {
    vector3_type p = vector3_type( 1.0, 1.0,  -1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, c, a);
    BOOST_CHECK_CLOSE( sign_p, 0.70710678118654752440084436210485, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, b, a);
    BOOST_CHECK_CLOSE( sign_p, 0.70710678118654752440084436210485, 0.01 );
  }
  // Back side of BC voronoi plane
  {
    vector3_type p = vector3_type( 0.0, 0.0, -1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, c, a);
    BOOST_CHECK_CLOSE( sign_p, -0.70710678118654752440084436210485, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, b, a);
    BOOST_CHECK_CLOSE( sign_p, -0.70710678118654752440084436210485, 0.01 );
  }
  // In BC voronoi plane
  {
    vector3_type p = vector3_type( 0.5, 0.5, -1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, b, c, a);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );

    sign_p = OpenTissue::gjk::detail::signed_distance_to_edge_face_voronoi_plane(p, c, b, a);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }

}

BOOST_AUTO_TEST_SUITE_END();


#include <tdp/testing/testing.h>
#include <iostream>
#include <Eigen/Dense>
#include <tdp/manifold/SO3.h>
#include <tdp/manifold/rotation.h>
#include <tdp/manifold/SE3.h>

#include <tdp/data/managed_image.h>
#include <tdp/eigen/dense.h>
#include <tdp/preproc/pc.h>

using namespace tdp;

TEST(SE3, setup) {

  SE3d T;
  std::cout << T << std::endl;

  double theta = 15.*M_PI/180.;
  Eigen::Matrix4d Tmu_;
  Tmu_ << 1, 0, 0, 0,
         0, cos(theta), sin(theta), 0,
         0, -sin(theta), cos(theta), 0,
           0,0,0,1;
  SE3d Tmu(Tmu_);

  std::cout << Tmu << std::endl;
  std::cout << T*Tmu << std::endl;
  std::cout << T << std::endl;
  std::cout << Tmu*T << std::endl;

  std::cout << Tmu.Inverse()*Tmu << std::endl;

//  SE3f T1(SO3f::Rx(ToRad(10.)), Eigen::Vector3f(0,0,1));
//  std::cout << T1 << std::endl;
//  std::cout << T1*Eigen::Vector3f(1,0,0) << std::endl;
//  std::cout << T1*Eigen::Vector3f(0,1,0) << std::endl;
//
//  SE3f T2(SO3f::Rx(ToRad(10.)));

//  std::cout << T-Tmu << std::endl;
//  Eigen::Matrix<double,6,1> w = T-Tmu;
//  std::cout << Tmu.Exp(w) << std::endl;
//  std::cout << Tmu-T << std::endl;

}

TEST(SE3, transform) {

  const float eps = 1e-5;
  for (size_t i=0; i<10000; ++i) {
    Eigen::Matrix<float,3,1> p0 = Eigen::Matrix<float,3,1>::Random();
    SE3f T = SE3f::Random();
    Eigen::Matrix4f Tmat = T.matrix();
    Eigen::Matrix4f TmatInv = Tmat.inverse();

    Eigen::Vector3f p1 = TmatInv.topLeftCorner(3,3)*p0 + TmatInv.topRightCorner(3,1);
    Eigen::Vector3f p2 = T.Inverse()*p0;
    ASSERT_TRUE(IsAppox(p1, p2, eps));

    p1 = Tmat.topLeftCorner(3,3)*p0 + Tmat.topRightCorner(3,1);
    p2 = T*p0;
    ASSERT_TRUE(IsAppox(p1, p2, eps));

  }
}

TEST(SE3, inverse) {

  const float eps = 1e-5;
  for (size_t i=0; i<10000; ++i) {
    Eigen::Matrix<float,6,1> x0 = Eigen::Matrix<float,6,1>::Random();
    SE3f T = SE3f::Exp_(x0);
    Eigen::Matrix4f Tmat = T.matrix();

    Eigen::Matrix4f TmatInv = Tmat.inverse();
    Eigen::Matrix4f Tinv = T.Inverse().matrix();
    ASSERT_TRUE(IsAppox(TmatInv, Tinv, eps));

    SE3f Tse3Inv = T.Inverse();
    Eigen::Matrix4f Tinvinvmat = Tse3Inv.Inverse().matrix();
    ASSERT_TRUE(IsAppox(Tinvinvmat, Tmat, eps));

  }
}

TEST(SE3, expLog) {

  const float eps = 1e-5;
  for (size_t i=0; i<10000; ++i) {
    Eigen::Matrix<float,6,1> x0 = Eigen::Matrix<float,6,1>::Random();
    SE3f T = SE3f::Exp_(x0);
    Eigen::Matrix<float,6,1> x1 = SE3f::Log_(T);
    ASSERT_TRUE(IsAppox(x0, x1, eps));

//    Eigen::Matrix4f Tmat = T.matrix();
//
//    Eigen::Matrix4f TmatInv = Tmat.inverse();
//    Eigen::Matrix4f Tinv = T.Inverse().matrix();
//    ASSERT_TRUE(IsAppox(TmatInv, Tinv, eps));
//
//    SE3f Tse3Inv = T.Inverse();
//    Eigen::Matrix4f Tinvinvmat = Tse3Inv.Inverse().matrix();
//    ASSERT_TRUE(IsAppox(Tinvinvmat, Tmat, eps));

  }
}

TEST(SE3, composition) {

  const float eps = 1e-5;
  for (size_t i=0; i<1000; ++i) {
    SE3f Tw0 = SE3f::Random();
    SE3f Tw1 = SE3f::Random();
    Eigen::Matrix4f Tw0mat = Tw0.matrix();
    Eigen::Matrix4f Tw1mat = Tw1.matrix();

    SE3f T01 = Tw0.Inverse() * Tw1;
    Eigen::Matrix4f T01mat = Tw0mat.inverse()*Tw1mat;

    Eigen::Matrix4f T01_mat = T01.matrix();
    ASSERT_TRUE(IsAppox(T01mat,T01_mat,eps));

    SE3f Tw0w1 = Tw0 * Tw1;
    Eigen::Matrix4f Tw0w1mat = Tw0mat*Tw1mat;
    ASSERT_TRUE(Tw0w1mat.isApprox(Tw0w1.matrix(),eps));

  }

  SE3f Tw0;
  Eigen::Matrix4f Tw0mat = Eigen::Matrix4f::Identity();
  SE3f Tw1;
  Eigen::Matrix4f Tw1mat = Eigen::Matrix4f::Identity();
  for (size_t i=0; i<1000; ++i) {
    Eigen::Matrix<float,6,1> x0 = 1e-3*Eigen::Matrix<float,6,1>::Random();
    Tw0 = Tw0 * SE3f::Exp_(x0);
    Tw0mat = Tw0mat * SE3f::Exp_(x0).matrix();
    ASSERT_TRUE(Tw0mat.isApprox(Tw0.matrix(),eps));

    Tw1 = SE3f::Exp_(x0) * Tw1;
    Tw1mat = SE3f::Exp_(x0).matrix() * Tw1mat;
    ASSERT_TRUE(Tw1mat.isApprox(Tw1.matrix(),eps));
  }

}

TEST(SE3, exp) {

  const float eps = 1e-5;

  Eigen::Matrix<float,6,1> x0;
  x0 << 0,0,ToRad(10.),0,0.1,0.1;
  SE3f T0 = SE3f::Exp_(x0);
  Eigen::Matrix<float,6,1> x1 = SE3f::Log_(T0);
  std::cout << x0.transpose() << std::endl;
  std::cout << x1.transpose() << std::endl;

  ASSERT_NEAR(x0(0), x1(0), eps);
  ASSERT_NEAR(x0(1), x1(1), eps);
  ASSERT_NEAR(x0(2), x1(2), eps);
  ASSERT_NEAR(x0(3), x1(3), eps);
  ASSERT_NEAR(x0(4), x1(4), eps);
  ASSERT_NEAR(x0(5), x1(5), eps);

  x0 << 0,0,0.,0,0.1,0.1;
  T0 = SE3f::Exp_(x0);
  x1 = SE3f::Log_(T0);
  std::cout << x0.transpose() << std::endl;
  std::cout << x1.transpose() << std::endl;

  ASSERT_NEAR(x0(0), x1(0), eps);
  ASSERT_NEAR(x0(1), x1(1), eps);
  ASSERT_NEAR(x0(2), x1(2), eps);
  ASSERT_NEAR(x0(3), x1(3), eps);
  ASSERT_NEAR(x0(4), x1(4), eps);
  ASSERT_NEAR(x0(5), x1(5), eps);

  for (size_t i=0; i<10000; ++i) {
    x0 = Eigen::Matrix<float,6,1>::Random();
    T0 = SE3f::Exp_(x0);
    x1 = SE3f::Log_(T0);

    ASSERT_NEAR(x0(0), x1(0), eps);
    ASSERT_NEAR(x0(1), x1(1), eps);
    ASSERT_NEAR(x0(2), x1(2), eps);
    ASSERT_NEAR(x0(3), x1(3), eps);
    ASSERT_NEAR(x0(4), x1(4), eps);
    ASSERT_NEAR(x0(5), x1(5), eps);

  }

}

#ifdef CUDA_FOUND
TEST(SE3, gpu) {

  const float eps = 1e-5;

  for (size_t it=0; it<100; ++it) {
    SE3f T = SE3f::Random();
//    SO3f R (Eigen::Quaternion<float,Eigen::DontAlign>(T.rotation().vector()));
    SO3f R (T.rotation());
//    std::cout << T << std::endl;
//    std::cout << T.rotation() << std::endl;
//    std::cout << R << std::endl;

    ManagedHostImage<Vector3fda> x(1000,1);
    ManagedHostImage<Vector3fda> xBefore(1000,1);
    ManagedHostImage<Vector3fda> xAfter(1000,1);
    ManagedHostImage<Vector3fda> xAfterInv(1000,1);
    ManagedHostImage<Vector3fda> xAfterInvRot(1000,1);
    ManagedHostImage<Vector3fda> xAfterRot(1000,1);
    xAfter.Fill(Vector3fda::Zero());
    xAfterInv.Fill(Vector3fda::Zero());
    xAfterInvRot.Fill(Vector3fda::Zero());
    xAfterRot.Fill(Vector3fda::Zero());

    ManagedDeviceImage<Vector3fda> cuX(1000,1);
    ManagedDeviceImage<Vector3fda> cuXinv(1000,1);
    ManagedDeviceImage<Vector3fda> cuXinvRot(1000,1);
    ManagedDeviceImage<Vector3fda> cuXrot(1000,1);

    cudaMemset(cuX.ptr_, 0, cuX.SizeBytes());
    cudaMemset(cuXinv.ptr_, 0, cuXinv.SizeBytes());
    cudaMemset(cuXinvRot.ptr_, 0, cuXinvRot.SizeBytes());
    cudaMemset(cuXrot.ptr_, 0, cuXrot.SizeBytes());

    for (size_t i=0; i<1000; ++i) {
      x[i] = Vector3fda::Random();
      xBefore[i] = x[i];
      xAfter[i] = T*x[i];
      xAfterInv[i] = T.Inverse()*x[i];
      xAfterRot[i] = T.rotation()*x[i];
      xAfterInvRot[i] = T.rotation().Inverse()*x[i];
    }
    cuX.CopyFrom(x, cudaMemcpyHostToDevice);
    cuXinv.CopyFrom(x, cudaMemcpyHostToDevice);
    cuXinvRot.CopyFrom(x, cudaMemcpyHostToDevice);
    cuXrot.CopyFrom(x, cudaMemcpyHostToDevice);

    TransformPc(T, cuX);
    InverseTransformPc(T, cuXinv);
    InverseTransformPc(R, cuXinvRot);
    TransformPc(R, cuXrot);

    x.CopyFrom(cuX, cudaMemcpyDeviceToHost);
    for (size_t i=0; i<1000; ++i) {
      ASSERT_TRUE(IsAppox(x[i],xAfter[i], eps));
    }
    x.CopyFrom(cuXrot, cudaMemcpyDeviceToHost);
    for (size_t i=0; i<1000; ++i) {
      ASSERT_TRUE(IsAppox(x[i],xAfterRot[i], eps));
    }
    x.CopyFrom(cuXinvRot, cudaMemcpyDeviceToHost);
    for (size_t i=0; i<1000; ++i) {
      ASSERT_TRUE(IsAppox(x[i],xAfterInvRot[i], eps));
    }
    x.CopyFrom(cuXinv, cudaMemcpyDeviceToHost);
    for (size_t i=0; i<1000; ++i) {
      if(!IsAppox(x[i],xAfterInv[i], eps)) {
        std::cout << xBefore[i].transpose() << std::endl;
      }
      ASSERT_TRUE(IsAppox(x[i],xAfterInv[i], eps));
    }
  }
}
#endif

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


/**
 *  .file test/oglplus/math.cpp
 *  .brief Test case for math utilities.
 *
 *  .author Matus Chochlik
 *
 *  Copyright 2011-2015 Matus Chochlik. Distributed under the Boost
 *  Software License, Version 1.0. (See accompanying file
 *  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE OGLPLUS_math
#include <boost/test/unit_test.hpp>

#include <oglplus/gl.hpp>
#include <oglplus/math/constants.hpp>

BOOST_AUTO_TEST_SUITE(math)

BOOST_AUTO_TEST_CASE(math_Pi)
{
	BOOST_CHECK_CLOSE(oglplus::math::Pi(), 3.14159265, 0.0001);
}

BOOST_AUTO_TEST_CASE(math_2Pi_eq_TwoPi)
{
	BOOST_CHECK(2*oglplus::math::Pi() == oglplus::math::TwoPi());
}

BOOST_AUTO_TEST_CASE(math_2HalfPi_eq_Pi)
{
	BOOST_CHECK(2*oglplus::math::HalfPi() == oglplus::math::Pi());
}

BOOST_AUTO_TEST_CASE(math_4HalfPi_eq_TwoPi)
{
	BOOST_CHECK(4*oglplus::math::HalfPi() == oglplus::math::TwoPi());
}

BOOST_AUTO_TEST_SUITE_END()


/*
(c) 2019 M. Werner - Part of the GIS++ tutorial
- https://www.martinwerner.de/teaching/spatial-cpp
- https://github.com/mwernerds/spatial-cpp

Program: R-Tree
Compile: g++ -I $(BOOST_DIR) -Ofast -march=native  -Wall -std=c++11  -o 03_rtree 03_rtree.cpp
*/

#include<iostream>
#include<fstream>
#include <boost/geometry.hpp>
#include <boost/algorithm/string.hpp>
#include<chrono>

#include<set>

#include <boost/range/adaptor/indexed.hpp>
using  boost::adaptors::indexed;
#include <boost/range/adaptor/transformed.hpp>
using  boost::adaptors::transformed;
#include <boost/function_output_iterator.hpp>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

typedef bg::model::point<double, 2, bg::cs::cartesian> point;
typedef bg::model::box<point> box;
typedef bg::model::linestring<point> linestring;
typedef bg::model::polygon<point, false, false> polygon; // ccw, open polygon
typedef bg::model::multi_polygon<polygon> multi_polygon; // ccw, open polygon
typedef std::pair<box, size_t> value; // <- this is what the R-tree will hold

typedef bgi::rtree< value, bgi::rstar<16, 4> > rtree;

std::vector<std::pair<polygon, size_t>> dataset;

struct value_maker
{
    template<typename T>
    inline value operator()(T const& v) const
    {
	box b;
	bg::envelope(v.value().first,b);
        return value(b, v.index());
    }
};

std::ostream &operator<< (std::ostream &os, box &b)
{
    os << "(" << bg::get<0>(b.min_corner()) << ";" << bg::get<1>(b.min_corner()) << ")" << "-->"
	  << "(" << bg::get<0>(b.max_corner()) << ";" << bg::get<1>(b.max_corner()) << ")" ;
    return os;
}

point  operator + (const point &p, const point &b)
{
    return bg::make<point>(bg::get<0>(p)+bg::get<0>(b),bg::get<1>(p)+bg::get<1>(b));

}
point  operator - (const point &p, const point &b)
{
    return bg::make<point>(bg::get<0>(p)-bg::get<0>(b),bg::get<1>(p)-bg::get<1>(b));

}

point random_point_in_box(box b)
{
    double tau1 = static_cast<double> (std::rand()) / RAND_MAX;
    double tau2 = static_cast<double> (std::rand()) / RAND_MAX;
    return bg::make<point>(
	bg::get<0>(b.min_corner())+ tau1 * (bg::get<0>(b.max_corner())-bg::get<0>(b.min_corner())),
	bg::get<1>(b.min_corner())+ tau2 * (bg::get<1>(b.max_corner())-bg::get<1>(b.min_corner())));

}

int main(int argc, char **argv)
{
    std::srand(std::time(0)); //use current time as seed for random generator

// Load the OSM polygons and explode each multipolygon into polygons to be added to the index.
    box roi(point(0,0),point(0,0));
    { // scope for timing
    auto start = std::chrono::high_resolution_clock::now();

    std::ifstream ifs("washington_dc_osm_buildings.wkt");
    std::string line;

    while(std::getline(ifs, line))
    {
	// split at ";"
	std::vector<std::string> entries;
	boost::split(entries, line, [](char c){return c == ';';});
	size_t osm_id = boost::lexical_cast<size_t>(entries[0]);

	// remove "
	 entries[1].erase(remove_if(entries[1].begin(), entries[1].end(), [](const char& c) {
        return c=='"';   }), entries[1].end());
	multi_polygon mp;
	bg::read_wkt(entries[1],mp);
	for (auto &p: mp) // each building part!
	{
	    bg::correct(p);
	    dataset.push_back(std::make_pair(p,osm_id));
	    if (bg::get<0>(roi.min_corner()) == 0){ // bad hack
		bg::envelope(p,roi);
	    }else{
		box q;
		bg::envelope(p,q);
		bg::expand(roi,q);
	    }
	}
    }
    std::cout << "Dataset contains " << dataset.size() << " polygons" << std::endl;
    std::cout << "MBR of dataset: " << roi << std::endl;
    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> diff = end-start;
    std::cout << " Load CSV in " << diff.count() << "seconds" << std::endl;
    } // loading scope
    // now load this into an R-tree: variant 1: sequential insert

    { // sequential r-tree population
    auto start = std::chrono::high_resolution_clock::now();
    rtree rt;

    for (const auto &d:dataset |indexed())
    {
	box b;
	bg::envelope(d.value().first, b); // create MBR
	rt.insert(value(b,d.index()));
    }

    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> diff = end-start;
    std::cout << " Sequential R-Tree in " << diff.count() << "seconds" << std::endl;
    }
    rtree rt2; // just to have it later
    // variant 2: bulk-load
    { // bulk load scope
    auto start = std::chrono::high_resolution_clock::now();
    rt2 = rtree (dataset | indexed()
                       | transformed(value_maker()));
    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> diff = end-start;
    std::cout << " Bulk-Load R-Tree in " << diff.count() << "seconds" << std::endl;
    } // bulk load scope

    // Let us now do a kNN (k = 10) query for a single random point
    std::vector<value> result;
    auto p = random_point_in_box(roi);
    // for reproducibilty:
    p = bg::make<point>(-76.8117, 38.812);
    std::cout << "Anchor: " << bg::wkt(p) << std::endl;
    rt2.query(bgi::nearest(p, 10), std::back_inserter(result));
    // two issues to be resolved: first, kNN is not ordered, second, r-tree knows bbox only
    std::sort(result.begin(), result.end(), [p](const value & a, const value & b){
	return bg::distance(dataset[a.second].first,p) < bg::distance(dataset[b.second].first,p );
    });

    for (const auto &v:result | indexed())
    {
	auto id = v.value().second;
	std::cout << v.index() << "\t" << dataset[id].second << "\t" << bg::distance (p, dataset[id].first) << std::endl;
    }

    // and again something for QGIS:
    // we will take a random point, then 10-nearest neighbors (BBOX), then double the distance for a range query.
    // with all these results, we will write a single CSV containing classified buildings.
    result.clear();
    point anchor = bg::make<point> (-76.99017,38.88970);
    std::cout << "Anchor: " << bg::wkt(anchor) << std::endl;
    rt2.query(bgi::nearest(anchor, 200), std::back_inserter(result));
    double radius = 0.03;

    box range_query_box;
    range_query_box.min_corner() = anchor - bg::make<point>(radius,radius);
    range_query_box.max_corner() = anchor + bg::make<point>(radius,radius);

    std::cout << "Range Query Box:" << range_query_box << std::endl;

    // function_output_iterator is a nice tool as well:
    std::set<size_t> knnids;
    std::ofstream ofs("range_knn.csv");
    ofs <<  std::setprecision(std::numeric_limits<double>::digits10);
    ofs << "wkt;role" << std::endl;
    // first write all kNN with their ID
    for (auto r:result)
    {
	const auto &item = dataset[r.second];
	ofs << bg::wkt(item.first) <<";" << 1 << std::endl;
	knnids.insert(r.second);
    }

    rt2.query(bgi::within(range_query_box), boost::make_function_output_iterator([&](value const& v)
    {
	const auto &item = dataset[v.second];
	if (knnids.find(v.second) == knnids.end()){
	   ofs << bg::wkt(item.first) << ";" << 2 << std::endl;
	}
    }
    ));

    return 0;
}


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  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 2 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, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if !defined(LOGNORMAL_HPP)
#define LOGNORMAL_HPP

#if defined(_MSC_VER)
#	pragma warning(disable: 4267)	// warning about loss of data when converting size_t to int
#endif

#include <cmath>
//#include "ncl/nxsdefs.h"

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

#include "probability_distribution.hpp"

namespace phycas
{

/*------------------------------------------------------------------------------------------------------------------------------------------------------------------
|	The Lognormal distribution with two parameters, the mean and standard deviation of the natural logarithm of the variable.
*/
class LognormalDistribution : public ProbabilityDistribution
	{
	public:
					LognormalDistribution();
					LognormalDistribution(double mean, double stddev);
					LognormalDistribution(const LognormalDistribution & other);
					~LognormalDistribution();

        LognormalDistribution * cloneAndSetLot(Lot * other) const;
        LognormalDistribution * Clone() const;
		bool		IsDiscrete() const;
		std::string	GetDistributionName() const;
		std::string	GetDistributionDescription() const;
		double		GetMean() const;
		double		GetVar() const;
		double		GetStdDev() const;
		double		GetCDF(double x) const;
		double		Sample() const;
		double		GetLnPDF(double x) const;
		double		GetRelativeLnPDF(double x) const;
		void		SetMeanAndVariance(double mean, double var);

	protected:
		void 			initialize(double m, double s);
		void			ComputeLnConst();

	protected:
		double		logmean;			/**< the mean parameter of the lognormal distribution */
		double		logsd;				/**< the standard deviation parameter of the lognormal distribution */
		double		logsd_squared;		/**< the square of the logsd parameter (i.e. the variance of the log of this lognormal random variable) */
		double		ln_const;			/**< the natural logarithm of the constant part of the density function */
		double		pi_const;			/**< precalculated (in constructor) value of pi */
		double		sqrt2_const;		/**< precalculated (in constructor) value of sqrt(2.0) */

	};

} // namespace phycas

#endif


/**
 * @file performance_functions_test.cpp
 * @author Marcus Edel
 *
 *  Tests for the various performance functions.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/performance_functions/mse_function.hpp>
#include <mlpack/methods/ann/performance_functions/cee_function.hpp>
#include <mlpack/methods/ann/performance_functions/sse_function.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(PerformanceFunctionsTest);

// Test the mean squared error performance function.
BOOST_AUTO_TEST_CASE(MeanSquaredErrorTest)
{
  arma::colvec input("1.0 0.0 1.0 0.0 -1.0 0.0 -1.0 0.0");
  arma::colvec target = arma::zeros<arma::colvec>(8);

  BOOST_REQUIRE_EQUAL(MeanSquaredErrorFunction::Error(input, target), 0.5);
}

// Test the cross entropy performance function.
BOOST_AUTO_TEST_CASE(CrossEntropyErrorTest)
{
  arma::colvec input;
  input << std::exp(-2.0) << std::exp(-1.0);
  arma::colvec target = arma::ones<arma::colvec>(2);

  BOOST_REQUIRE_EQUAL(CrossEntropyErrorFunction<>::Error(input, target), 3);
}

// Test the sum squared error performance function.
BOOST_AUTO_TEST_CASE(SumSquaredErrorTest)
{
  arma::colvec input("1.0 0.0 1.0 0.0 -1.0 0.0 -1.0 0.0");
  arma::colvec target = arma::zeros<arma::colvec>(8);

  BOOST_REQUIRE_EQUAL(SumSquaredErrorFunction::Error(input, target), 4);
}

BOOST_AUTO_TEST_SUITE_END();


#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d.hpp>
#include <iostream>
#include <armadillo>
#include "highgui.h"

using namespace std;
using namespace cv;

void detect(cv::Mat &frame) {
	int h1 = 100;
	int h2 = 179;
	int s1 = 125;
	int s2 = 255;
	int v1 = 50;
	int v2 = 170;
	cv::Mat hsv;
	cv::cvtColor(frame, hsv, COLOR_BGR2HSV);
	cv::Mat hsv2mask, hsv2;
	cv::inRange(hsv, cv::Scalar(h1, s1, v1), cv::Scalar(h2, s2, v2), hsv2mask);

	arma::mat I;
	cvt_opencv2arma(hsv2mask, I);
	if (arma::accu(I) < 10.0) {
		return;
	}
	arma::rowvec r = arma::sum(I, 0);
	arma::colvec c = arma::sum(I, 1);
	double midx = arma::sum(r * arma::cumsum(arma::ones<arma::vec>(r.n_elem))) / arma::sum(r);
	double midy = arma::sum(c % arma::cumsum(arma::ones<arma::vec>(c.n_elem))) / arma::sum(c);

	double covx = sqrt(arma::sum(r * arma::square(arma::cumsum(arma::ones<arma::vec>(r.n_elem)) - midx)) / arma::sum(r));
	double covy = sqrt(arma::sum(c % arma::square(arma::cumsum(arma::ones<arma::vec>(c.n_elem)) - midy)) / arma::sum(c));
	double estw = covx;
	double esth = estw;
	estw *= 3; // strange constants
	esth *= 3;

	arma::vec pos = arma::vec({ midx, midy });
	double width = covx * 3;
	cout << "pos: " << pos << endl;
	cout << "width: " << width << endl;
}

int main(int argc, char *argv[]) {
	if (argc != 8) {
		cout << "usage: ./test img h1 h2 s1 s2 v1 v2\n";
		return 1;
	}
	int h1 = atoi(argv[2]);
	int h2 = atoi(argv[3]);
	int s1 = atoi(argv[4]);
	int s2 = atoi(argv[5]);
	int v1 = atoi(argv[6]);
	int v2 = atoi(argv[7]);

	// convert to hsv
	VideoCapture cam(1);

	arma::vec xbuf(10, arma::fill::zeros);
	arma::vec ybuf(10, arma::fill::zeros);
	int xind = 0, yind = 0;
	bool stable = false;

	while (1) {
		Mat img;
		cam.read(img);
		Mat hsv;
		cvtColor(img, hsv, COLOR_BGR2HSV);

		// in range masking
		Mat hsv2mask, hsv2;
		inRange(hsv, Scalar(h1, s1, v1), Scalar(h2, s2, v2), hsv2mask);
		//hsv.copyTo(hsv2, hsv2mask);

		arma::mat I;
		cvt_opencv2arma(hsv2mask, I);
		arma::rowvec r = arma::sum(I, 0);
		arma::colvec c = arma::sum(I, 1);
		double midx = arma::sum(r * arma::cumsum(arma::ones<arma::vec>(r.n_elem))) / arma::sum(r);
		double midy = arma::sum(c % arma::cumsum(arma::ones<arma::vec>(c.n_elem))) / arma::sum(c);
		stable = true;
		if (midx < 1 || (midx > (int)I.n_cols - 1) || arma::sum(r) == 0) {
			midx = 0;
		}
		if (midy < 1 || (midy > (int)I.n_rows - 1) || arma::sum(c) == 0) {
			midy = 0;
		}

		// remove noise
		if (midx == 0 || midy == 0) {
			stable = false;
		}
		xbuf[xind] = midx;
		ybuf[yind] = midy;
		xind = (xind + 1) % (int)xbuf.n_elem;
		yind = (yind + 1) % (int)ybuf.n_elem;
		midx = arma::mean(xbuf);
		midy = arma::mean(ybuf);
		if (sqrt(arma::var(xbuf) * arma::var(xbuf) + arma::var(ybuf) + arma::var(ybuf)) > 70.0) {
			stable = false;
		}

		Vec3b color(255, 0, 0);
		cout << "arma:: " << midx << ", " << midy << endl;
		cout << "stable: " << stable << endl;
		circle(img, Point((int)midx, (int)midy), 4, color, 0);

		double covx = sqrt(arma::sum(r * arma::square(arma::cumsum(arma::ones<arma::vec>(r.n_elem)) - midx)) / arma::sum(r));
		double covy = sqrt(arma::sum(c % arma::square(arma::cumsum(arma::ones<arma::vec>(c.n_elem)) - midy)) / arma::sum(c));
		double estw = covx * 3;
		double esth = covy * 3;

		rectangle(img, Rect(midx-estw/2,midy-esth/2,estw,esth), Scalar(0, 0, 255), 2);

		// use histogram binning to get the center

		// create blob params
		/*SimpleBlobDetector::Params params;

		// Change thresholds
		params.minThreshold = 10;
		params.maxThreshold = 200;

		// Filter by Area.
		params.filterByArea = true;
		params.minArea = 1500;

		// Filter by Circularity
		params.filterByCircularity = true;
		params.minCircularity = 0.1;

		// Filter by Convexity
		params.filterByConvexity = true;
		params.minConvexity = 0.87;

		// Filter by Inertia
		params.filterByInertia = true;
		params.minInertiaRatio = 0.01;

		Ptr<SimpleBlobDetector> blob = SimpleBlobDetector::create(params);
		vector<KeyPoint> kp;
		Mat des;
		Mat hsvd = hsv2mask * -0.75 + 255;
		blob->detect(hsvd, kp);

		cout << "found " << kp.size() << " matches\n";

		Mat kpimg;
		drawKeypoints(img, kp, kpimg, Scalar(0, 0, 255), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);*/

		imshow("oldhsv", hsv);
		imshow("newhsv", hsv2mask);
		imshow("img", img);
		if (waitKey(30) & 0xff == 'Q') {
			break;
		}
	}
	return 1;
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/mult.hpp>
#include <boost/hana/one.hpp>

#include "matrix/comparable.hpp"
#include "matrix/ring.hpp"
namespace hana = boost::hana;
using namespace cppcon;

int main() {
    // mult
    {
        auto a = matrix(
            row(1, 2, 3),
            row(4, 5, 6)
        );

        auto b = matrix(
            row(1, 2),
            row(3, 4),
            row(5, 6)
        );

        BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
            hana::mult(a, b),
            matrix(
                row(1*1 + 2*3 + 5*3, 1*2 + 2*4 + 3*6),
                row(4*1 + 3*5 + 5*6, 4*2 + 5*4 + 6*6)
            )
        ));
    }

    // one
    {
        BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
            hana::one<Matrix<1, 1>>(),
            matrix(
                row(1)
            )
        ));

        BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
            hana::one<Matrix<2, 2>>(),
            matrix(
                row(1, 0),
                row(0, 1)
            )
        ));

        BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
            hana::one<Matrix<3, 3>>(),
            matrix(
                row(1, 0, 0),
                row(0, 1, 0),
                row(0, 0, 1)
            )
        ));

        BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
            hana::one<Matrix<4, 4>>(),
            matrix(
                row(1, 0, 0, 0),
                row(0, 1, 0, 0),
                row(0, 0, 1, 0),
                row(0, 0, 0, 1)
            )
        ));

        BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
            hana::one<Matrix<4, 5>>(),
            matrix(
                row(1, 0, 0, 0, 0),
                row(0, 1, 0, 0, 0),
                row(0, 0, 1, 0, 0),
                row(0, 0, 0, 1, 0)
            )
        ));

        BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
            hana::one<Matrix<5, 4>>(),
            matrix(
                row(1, 0, 0, 0),
                row(0, 1, 0, 0),
                row(0, 0, 1, 0),
                row(0, 0, 0, 1),
                row(0, 0, 0, 0)
            )
        ));
    }
}


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_SPECIAL_LEGENDRE_HPP
#define BOOST_MATH_SPECIAL_LEGENDRE_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <utility>
#include <vector>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/math/tools/config.hpp>

namespace boost{
namespace math{

// Recurrance relation for legendre P and Q polynomials:
template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
   legendre_next(unsigned l, T1 x, T2 Pl, T3 Plm1)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   return ((2 * l + 1) * result_type(x) * result_type(Pl) - l * result_type(Plm1)) / (l + 1);
}

namespace detail{

// Implement Legendre P and Q polynomials via recurrance:
template <class T, class Policy>
T legendre_imp(unsigned l, T x, const Policy& pol, bool second = false)
{
   static const char* function = "boost::math::legrendre_p<%1%>(unsigned, %1%)";
   // Error handling:
   if((x < -1) || (x > 1))
      return policies::raise_domain_error<T>(
         function,
         "The Legendre Polynomial is defined for"
         " -1 <= x <= 1, but got x = %1%.", x, pol);

   T p0, p1;
   if(second)
   {
      // A solution of the second kind (Q):
      p0 = (boost::math::log1p(x, pol) - boost::math::log1p(-x, pol)) / 2;
      p1 = x * p0 - 1;
   }
   else
   {
      // A solution of the first kind (P):
      p0 = 1;
      p1 = x;
   }
   if(l == 0)
      return p0;

   unsigned n = 1;

   while(n < l)
   {
      std::swap(p0, p1);
      p1 = boost::math::legendre_next(n, x, p0, p1);
      ++n;
   }
   return p1;
}

template <class T, class Policy>
T legendre_p_prime_imp(unsigned l, T x, const Policy& pol, T* Pn
#ifdef BOOST_NO_CXX11_NULLPTR
   = 0
#else
   = nullptr
#endif
)
{
   static const char* function = "boost::math::legrendre_p_prime<%1%>(unsigned, %1%)";
   // Error handling:
   if ((x < -1) || (x > 1))
      return policies::raise_domain_error<T>(
         function,
         "The Legendre Polynomial is defined for"
         " -1 <= x <= 1, but got x = %1%.", x, pol);

   if (l == 0)
    {
        if (Pn)
        {
           *Pn = 1;
        }
        return 0;
    }
    T p0 = 1;
    T p1 = x;
    T p_prime;
    bool odd = l & 1;
    // If the order is odd, we sum all the even polynomials:
    if (odd)
    {
        p_prime = p0;
    }
    else // Otherwise we sum the odd polynomials * (2n+1)
    {
        p_prime = 3*p1;
    }

    unsigned n = 1;
    while(n < l - 1)
    {
       std::swap(p0, p1);
       p1 = boost::math::legendre_next(n, x, p0, p1);
       ++n;
       if (odd)
       {
          p_prime += (2*n+1)*p1;
          odd = false;
       }
       else
       {
           odd = true;
       }
    }
    // This allows us to evaluate the derivative and the function for the same cost.
    if (Pn)
    {
        std::swap(p0, p1);
        *Pn = boost::math::legendre_next(n, x, p0, p1);
    }
    return p_prime;
}

template <class T, class Policy>
struct legendre_p_zero_func
{
   int n;
   const Policy& pol;

   legendre_p_zero_func(int n_, const Policy& p) : n(n_), pol(p) {}

   std::pair<T, T> operator()(T x) const
   {
      T Pn;
      T Pn_prime = detail::legendre_p_prime_imp(n, x, pol, &Pn);
      return std::pair<T, T>(Pn, Pn_prime);
   };
};

template <class T, class Policy>
std::vector<T> legendre_p_zeros_imp(int n, const Policy& pol)
{
    using std::cos;
    using std::sin;
    using std::ceil;
    using std::sqrt;
    using boost::math::constants::pi;
    using boost::math::constants::half;
    using boost::math::tools::newton_raphson_iterate;

    BOOST_ASSERT(n >= 0);
    std::vector<T> zeros;
    if (n == 0)
    {
        // There are no zeros of P_0(x) = 1.
        return zeros;
    }
    int k;
    if (n & 1)
    {
        zeros.resize((n-1)/2 + 1, std::numeric_limits<T>::quiet_NaN());
        zeros[0] = 0;
        k = 1;
    }
    else
    {
        zeros.resize(n/2, std::numeric_limits<T>::quiet_NaN());
        k = 0;
    }
    T half_n = ceil(n*half<T>());

    while (k < (int)zeros.size())
    {
        // Bracket the root: Szego:
        // Gabriel Szego, Inequalities for the Zeros of Legendre Polynomials and Related Functions, Transactions of the American Mathematical Society, Vol. 39, No. 1 (1936)
        T theta_nk =  ((half_n - half<T>()*half<T>() - static_cast<T>(k))*pi<T>())/(static_cast<T>(n)+half<T>());
        T lower_bound = cos( (half_n - static_cast<T>(k))*pi<T>()/static_cast<T>(n + 1));
        T cos_nk = cos(theta_nk);
        T upper_bound = cos_nk;
        // First guess follows from:
        //  F. G. Tricomi, Sugli zeri dei polinomi sferici ed ultrasferici, Ann. Mat. Pura Appl., 31 (1950), pp. 93–97;
        T inv_n_sq = 1/static_cast<T>(n*n);
        T sin_nk = sin(theta_nk);
        T x_nk_guess = (1 - inv_n_sq/static_cast<T>(8) + inv_n_sq /static_cast<T>(8*n) - (inv_n_sq*inv_n_sq/384)*(39  - 28 / (sin_nk*sin_nk) ) )*cos_nk;

        boost::uintmax_t number_of_iterations = policies::get_max_root_iterations<Policy>();

        legendre_p_zero_func<T, Policy> f(n, pol);

        const T x_nk = newton_raphson_iterate(f, x_nk_guess,
                                              lower_bound, upper_bound,
                                              policies::digits<T, Policy>(),
                                              number_of_iterations);

        BOOST_ASSERT(lower_bound < x_nk);
        BOOST_ASSERT(upper_bound > x_nk);
        zeros[k] = x_nk;
        ++k;
    }
    return zeros;
}

} // namespace detail

template <class T, class Policy>
inline typename boost::enable_if_c<policies::is_policy<Policy>::value, typename tools::promote_args<T>::type>::type
   legendre_p(int l, T x, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   static const char* function = "boost::math::legendre_p<%1%>(unsigned, %1%)";
   if(l < 0)
      return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_imp(-l-1, static_cast<value_type>(x), pol, false), function);
   return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_imp(l, static_cast<value_type>(x), pol, false), function);
}

template <class T, class Policy>
inline typename boost::enable_if_c<policies::is_policy<Policy>::value, typename tools::promote_args<T>::type>::type
   legendre_p_prime(int l, T x, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   static const char* function = "boost::math::legendre_p_prime<%1%>(unsigned, %1%)";
   if(l < 0)
      return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_p_prime_imp(-l-1, static_cast<value_type>(x), pol), function);
   return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_p_prime_imp(l, static_cast<value_type>(x), pol), function);
}

template <class T>
inline typename tools::promote_args<T>::type
   legendre_p(int l, T x)
{
   return boost::math::legendre_p(l, x, policies::policy<>());
}

template <class T>
inline typename tools::promote_args<T>::type
   legendre_p_prime(int l, T x)
{
   return boost::math::legendre_p_prime(l, x, policies::policy<>());
}

template <class T, class Policy>
inline std::vector<T> legendre_p_zeros(int l, const Policy& pol)
{
    if(l < 0)
        return detail::legendre_p_zeros_imp<T>(-l-1, pol);

    return detail::legendre_p_zeros_imp<T>(l, pol);
}

template <class T>
inline std::vector<T> legendre_p_zeros(int l)
{
   return boost::math::legendre_p_zeros<T>(l, policies::policy<>());
}

template <class T, class Policy>
inline typename boost::enable_if_c<policies::is_policy<Policy>::value, typename tools::promote_args<T>::type>::type
   legendre_q(unsigned l, T x, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_imp(l, static_cast<value_type>(x), pol, true), "boost::math::legendre_q<%1%>(unsigned, %1%)");
}

template <class T>
inline typename tools::promote_args<T>::type
   legendre_q(unsigned l, T x)
{
   return boost::math::legendre_q(l, x, policies::policy<>());
}

// Recurrence for associated polynomials:
template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
   legendre_next(unsigned l, unsigned m, T1 x, T2 Pl, T3 Plm1)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   return ((2 * l + 1) * result_type(x) * result_type(Pl) - (l + m) * result_type(Plm1)) / (l + 1 - m);
}

namespace detail{
// Legendre P associated polynomial:
template <class T, class Policy>
T legendre_p_imp(int l, int m, T x, T sin_theta_power, const Policy& pol)
{
   // Error handling:
   if((x < -1) || (x > 1))
      return policies::raise_domain_error<T>(
      "boost::math::legendre_p<%1%>(int, int, %1%)",
         "The associated Legendre Polynomial is defined for"
         " -1 <= x <= 1, but got x = %1%.", x, pol);
   // Handle negative arguments first:
   if(l < 0)
      return legendre_p_imp(-l-1, m, x, sin_theta_power, pol);
   if(m < 0)
   {
      int sign = (m&1) ? -1 : 1;
      return sign * boost::math::tgamma_ratio(static_cast<T>(l+m+1), static_cast<T>(l+1-m), pol) * legendre_p_imp(l, -m, x, sin_theta_power, pol);
   }
   // Special cases:
   if(m > l)
      return 0;
   if(m == 0)
      return boost::math::legendre_p(l, x, pol);

   T p0 = boost::math::double_factorial<T>(2 * m - 1, pol) * sin_theta_power;

   if(m&1)
      p0 *= -1;
   if(m == l)
      return p0;

   T p1 = x * (2 * m + 1) * p0;

   int n = m + 1;

   while(n < l)
   {
      std::swap(p0, p1);
      p1 = boost::math::legendre_next(n, m, x, p0, p1);
      ++n;
   }
   return p1;
}

template <class T, class Policy>
inline T legendre_p_imp(int l, int m, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING
   // TODO: we really could use that mythical "pow1p" function here:
   return legendre_p_imp(l, m, x, static_cast<T>(pow(1 - x*x, T(abs(m))/2)), pol);
}

}

template <class T, class Policy>
inline typename tools::promote_args<T>::type
   legendre_p(int l, int m, T x, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_p_imp(l, m, static_cast<value_type>(x), pol), "bost::math::legendre_p<%1%>(int, int, %1%)");
}

template <class T>
inline typename tools::promote_args<T>::type
   legendre_p(int l, int m, T x)
{
   return boost::math::legendre_p(l, m, x, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SPECIAL_LEGENDRE_HPP


#include "prime.hpp"

// Miller-Rabin prime test algorithm.
#include <boost/multiprecision/miller_rabin.hpp>
#include <random>

boost::multiprecision::cpp_int cryptb::prime::gen_random(const int num_bytes, random_engine& engine)
{
	boost::multiprecision::cpp_int candidate;
	std::mt19937_64 miller_rabin_engine(static_cast<std::mt19937_64::result_type>(engine.operator()(sizeof(std::mt19937_64::result_type))));
	do
	{
		candidate = engine.operator()(num_bytes);
		if (rand() % 2 == 0)
			candidate += 1;
		// 64 Should be enough. The higher the number of trials, the lower the probability is for a false positive.
		// Note: making this number lower will significantly improve performance.
	} while (boost::multiprecision::miller_rabin_test(candidate, 64, miller_rabin_engine) == false);
	return candidate;
}


#pragma once

/* Multi-key search */

#include "pbc/pbc.h"
#include <NTL/ZZ.h>
#include <string>
#include "main/ec.hh"

class mksum {
public:

    mksum();
    ~mksum();

	std::vector<NTL::ZZ> keygen() const;

    NTL::ZZ encrypt(const std::vector<NTL::ZZ> & pk, const NTL::ZZ & word);
    NTL::ZZ add(const std::vector<NTL::ZZ> & pk, const NTL::ZZ & c1, const NTL::ZZ & c2);
    NTL::ZZ decrypt(const std::vector<NTL::ZZ> & k, const NTL::ZZ & cipher);

	std::vector<NTL::ZZ> from_bytes(const std::string & serial);
    static std::string to_bytes(const std::vector<NTL::ZZ> & k);

	NTL::ZZ from_bytesN(const std::string & serial);
    static std::string to_bytes(const NTL::ZZ & k);
};


#include <armadillo>
#include <bitset>

using namespace arma;
using namespace std;

mat crearTuplas(vec datos,
                unsigned int longitudTupla)
{
    mat tuplas(datos.n_elem - longitudTupla + 1, longitudTupla);

    for (unsigned int i = 0; i < tuplas.n_rows; ++i) {
        rowvec tupla = datos(span(i, i + longitudTupla - 1)).t();
        tuplas.row(i) = tupla;
    }

    return tuplas;
}

vec normalizar(vec serieOriginal, vec serieACambiar)
{
    const double minimo = min(serieOriginal);
    const double rango = max(serieOriginal) - minimo;

    vec result = (serieACambiar - minimo) / rango;

    return result;
}

vec desnormalizar(vec serieOriginal, vec serieNormalizada)
{
    const double minimo = min(serieOriginal);
    const double rango = max(serieOriginal) - minimo;

    vec result = serieNormalizada * rango + minimo;

    return result;
}

struct VectorParticionado {
    vec entrenamiento;
    vec evaluacion;
    vec prueba;
};

struct ConjuntoDatos {
    mat tuplasEntrada;
    mat tuplasSalida;
};

struct Particion {
    ConjuntoDatos entrenamiento;
    ConjuntoDatos evaluacion;
    ConjuntoDatos prueba;
};

//mat agregarIndiceTemporal(const mat& tuplas)
//{
//    vec indice = linspace(1, tuplas.n_rows - 1, tuplas.n_rows);
//    indice = normalizar(indice, indice);

//    return join_horiz(indice, tuplas);
//}

ConjuntoDatos
agruparEntradasConSalidas(vector<vec> seriesEntrada,
                          vec serieSalida,
                          unsigned int retrasosEntrada,
                          unsigned int nSalidas)
{
    mat tuplasEntrada;
    for (vec& serie : seriesEntrada) {
        // Se eliminan los ultimos nSalidas elementos de cada serie.
        // Como se van a usar como salida deseada, no pueden formar
        // parte de las entradas.
        serie = serie.head(serie.n_elem - nSalidas);
        mat tuplas = crearTuplas(serie, retrasosEntrada);

        tuplasEntrada.insert_cols(tuplasEntrada.n_cols, tuplas);
    }

    // Los primeros retrasosEntrada elementos de serieSalida no pueden usarse como
    // salida deseada, ya que no va a haber retrasosEntrada elementos anteriores
    // para hacer la predicción.
    serieSalida = serieSalida.tail(serieSalida.n_elem - retrasosEntrada);
    const mat tuplasSalida = crearTuplas(serieSalida, nSalidas);

    if (tuplasEntrada.n_rows != tuplasSalida.n_rows)
        throw runtime_error("Esto no debería pasar");

    return {tuplasEntrada, tuplasSalida};
}

Particion
armarTuplas(vector<VectorParticionado> entradasParticionadas,
            VectorParticionado salidaParticionada,
            unsigned int retrasosEntrada,
            unsigned int nSalidas)
{
    vector<vec> seriesEntradaEntrenamiento;
    vector<vec> seriesEntradaEvaluacion;
    vector<vec> seriesEntradaPrueba;
    for (const VectorParticionado& vp : entradasParticionadas) {
        seriesEntradaEntrenamiento.push_back(vp.entrenamiento);
        seriesEntradaEvaluacion.push_back(vp.evaluacion);
        seriesEntradaPrueba.push_back(vp.prueba);
    }

    Particion result;
    result.entrenamiento = agruparEntradasConSalidas(seriesEntradaEntrenamiento,
                                                     salidaParticionada.entrenamiento,
                                                     retrasosEntrada,
                                                     nSalidas);
    result.evaluacion = agruparEntradasConSalidas(seriesEntradaEvaluacion,
                                                  salidaParticionada.evaluacion,
                                                  retrasosEntrada,
                                                  nSalidas);
    result.prueba = agruparEntradasConSalidas(seriesEntradaPrueba,
                                              salidaParticionada.prueba,
                                              retrasosEntrada,
                                              nSalidas);

    return result;
}

pair<vector<VectorParticionado>, VectorParticionado>
particionar(vector<vec> seriesEntrada,
            vec serieSalida)
{
    const int nDatos = serieSalida.n_elem;
    const int nDatosPrueba = nDatos * 0.1;
    const int nDatosEvaluacion = nDatos * 0.2;
    const int nDatosEntrenamiento = nDatos - nDatosPrueba - nDatosEvaluacion;

    vector<VectorParticionado> entradasParticionadas;
    for (const vec& entrada : seriesEntrada) {
        VectorParticionado entradaParticionada;
        entradaParticionada.entrenamiento = entrada.head(nDatosEntrenamiento);
        entradaParticionada.evaluacion = entrada(span(nDatosEntrenamiento,
                                                      nDatosEntrenamiento + nDatosEvaluacion - 1));
        entradaParticionada.prueba = entrada.tail(nDatosPrueba);

        entradasParticionadas.push_back(entradaParticionada);
    }

    VectorParticionado salidaParticionada;
    salidaParticionada.entrenamiento = serieSalida.head(nDatosEntrenamiento);
    salidaParticionada.evaluacion = serieSalida(span(nDatosEntrenamiento,
                                                     nDatosEntrenamiento + nDatosEvaluacion - 1));
    salidaParticionada.prueba = serieSalida.tail(nDatosPrueba);

    return make_pair(entradasParticionadas, salidaParticionada);
}

Particion
cargarTuplas(const vector<string>& rutasSeriesEntrada,
             const string& rutaSerieSalida,
             unsigned int retrasosEntrada,
             unsigned int nSalidas)
{
    vector<vec> seriesEntrada;
    for (const string& ruta : rutasSeriesEntrada) {
        seriesEntrada.push_back(vec{});
        seriesEntrada.back().load(ruta);
    }

    vec serieSalida;
    serieSalida.load(rutaSerieSalida);

    for (const vec& serie : seriesEntrada)
        if (serie.n_elem != serieSalida.n_elem)
            throw runtime_error("Las series de datos deben tener la misma longitud");

    // Normalizar todas las series de datos
    for (vec& v : seriesEntrada) {
        v = normalizar(v, v);
    }
    serieSalida = normalizar(serieSalida, serieSalida);

    //    if (agregarIndice) {
    //        vec indice = linspace(1, serieSalida.n_elem - 1, serieSalida.n_elem);
    //        indice = normalizar(indice, indice);

    //        seriesEntrada.push_back(indice);
    //    }

    vector<VectorParticionado> entradasParticionadas;
    VectorParticionado salidaParticionada;
    tie(entradasParticionadas, salidaParticionada) = particionar(seriesEntrada, serieSalida);

    if (salidaParticionada.prueba.n_elem < retrasosEntrada + nSalidas)
        throw runtime_error("No alcanzan los datos de prueba para armar una tupla con la dimensión requerida");

    Particion particion = armarTuplas(entradasParticionadas,
                                      salidaParticionada,
                                      retrasosEntrada,
                                      nSalidas);

    return particion;
}

template <unsigned int N>
vector<vector<string>>
subconjuntos(const vector<string>& conjunto)
{
    const int nElemResult = pow(2, conjunto.size());
    vector<vector<string>> result;

    if (N != conjunto.size())
        throw runtime_error("Pone bien los parámetros cacho");

    //    https://www.quora.com/How-do-I-generate-all-subsets-of-a-set-in-C++-iteratively
    bitset<N> setDeBits{0};

    for (int i = 0; i < nElemResult; ++i) {
        vector<string> subconjunto;

        for (unsigned int k = 0; k < conjunto.size(); k++) {
            if (setDeBits.test(k)) {
                subconjunto.push_back(conjunto.at(k));
            }
        }

        result.push_back(subconjunto);
        setDeBits = bitset<N>{setDeBits.to_ulong() + 1};
    }

    return result;
}