#include <codegenvar/Eigen>
#include <Eigen/Geometry>
#include <iostream>

using namespace codegenvar;

int main()
{
    const Symbol x("x"), y("y"), dx("dx"), dy("dy"), a("a");

    const Vec2 p(x, y);
    const Vec3 P = p.homogeneous();
    const Vec2 diff(dx, dy);

    std::cout << "p = " << p.transpose() << std::endl;
    std::cout << "translate by " << diff.transpose() << std::endl;
    std::cout << "rotate by " << a << std::endl << std::endl;
    std::cout << "P = " << P.transpose() << std::endl << std::endl;

    Mat23 t;
    t << Symbol(1), Symbol(0), diff(0),
         Symbol(0), Symbol(1), diff(1);

    std::cout << "p + diff = " << (p+diff).transpose() << std::endl << std::endl;
    std::cout << "t = " << std::endl << t << std::endl << std::endl;
    std::cout << "t*P = " << (t*P).transpose() << std::endl << std::endl;

    Mat3 T;
    T << t(0, 0), t(0, 1), t(0, 2),
         t(1, 0), t(1, 1), t(1, 2),
         Symbol(0), Symbol(0), Symbol(1);

    std::cout << "T = " << std::endl << T << std::endl << std::endl;
    std::cout << "T*P = " << (T*P).transpose() << std::endl << std::endl;

    Mat2 r;
    r <<  cos(a), -sin(a),
          sin(a),  cos(a);

    std::cout << "r = " << std::endl << r << std::endl << std::endl;
    std::cout << "r*p = " << (r*p).transpose() << std::endl << std::endl;

    Mat3 R;
    R << r(0, 0) , r(0, 1), Symbol(0),
         r(1, 0) , r(1, 1), Symbol(0),
         Symbol(0), Symbol(0), Symbol(1);

    std::cout << "R = " << std::endl << R << std::endl << std::endl;
    std::cout << "R*P = " << (R*P).transpose() << std::endl << std::endl;

    std::cout << "T*R = " << std::endl << T*R << std::endl << std::endl;
    std::cout << "T*R*P = " << (T*R*P).transpose() << std::endl << std::endl;

    std::cout << "R*T = " << std::endl << R*T << std::endl << std::endl;
    std::cout << "R*T*P = " << (R*T*P).transpose() << std::endl << std::endl;

    return 0;
}


#include <NTL/LLL.h>

NTL_CLIENT

int main()
{
   mat_ZZ B;
   long s;

#if 1
   cin >> B;
#else
   long i, j;
   long n;
   cerr << "n: ";
   cin >> n;

   long m;
   cerr << "m: ";
   cin >> m;

   long k;
   cerr << "k: ";
   cin >> k;

   B.SetDims(n, m);
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++) {
         RandomLen(B(i,j), k);
         if (RandomBnd(2)) negate(B(i,j), B(i,j));
      }

#endif

   mat_ZZ U, B0, B1, B2;

   B0 = B;

   double t;
   ZZ d;

   B = B0;
   cerr << "LLL_FP...";
   t = GetTime();
   s = LLL_FP(B, U, 0.99);
   cerr << (GetTime()-t) << "\n";
   mul(B1, U, B0);
   if (B1 != B) Error("bad LLLTest (1)");
   LLL(d, B, 90, 100);
   if (B1 != B) Error("bad LLLTest (2)");

   B = B0;
   cerr << "LLL_QP...";
   t = GetTime();
   s = LLL_QP(B, U, 0.99);
   cerr << (GetTime()-t) << "\n";
   mul(B1, U, B0);
   if (B1 != B) Error("bad LLLTest (1)");
   LLL(d, B, 90, 100);
   if (B1 != B) Error("bad LLLTest (2)");

   B = B0;
   cerr << "LLL_XD...";
   t = GetTime();
   s = LLL_XD(B, U, 0.99);
   cerr << (GetTime()-t) << "\n";
   mul(B1, U, B0);
   if (B1 != B) Error("bad LLLTest (1)");
   LLL(d, B, 90, 100);
   if (B1 != B) Error("bad LLLTest (2)");

   B = B0;
   cerr << "LLL_RR...";
   t = GetTime();
   s = LLL_RR(B, U, 0.99);
   cerr << (GetTime()-t) << "\n";
   mul(B1, U, B0);
   if (B1 != B) Error("bad LLLTest (1)");
   LLL(d, B, 90, 100);
   if (B1 != B) Error("bad LLLTest (2)");

   B = B0;
   cerr << "G_LLL_FP...";
   t = GetTime();
   s = G_LLL_FP(B, U, 0.99);
   cerr << (GetTime()-t) << "\n";
   mul(B1, U, B0);
   if (B1 != B) Error("bad LLLTest (1)");
   LLL(d, B, 90, 100);
   if (B1 != B) Error("bad LLLTest (2)");

   B = B0;
   cerr << "G_LLL_QP...";
   t = GetTime();
   s = G_LLL_QP(B, U, 0.99);
   cerr << (GetTime()-t) << "\n";
   mul(B1, U, B0);
   if (B1 != B) Error("bad LLLTest (1)");
   LLL(d, B, 90, 100);
   if (B1 != B) Error("bad LLLTest (2)");

   B = B0;
   cerr << "G_LLL_XD...";
   t = GetTime();
   s = G_LLL_XD(B, U, 0.99);
   cerr << (GetTime()-t) << "\n";
   mul(B1, U, B0);
   if (B1 != B) Error("bad LLLTest (1)");
   LLL(d, B, 90, 100);
   if (B1 != B) Error("bad LLLTest (2)");

   B = B0;
   cerr << "G_LLL_RR...";
   t = GetTime();
   s = G_LLL_RR(B, U, 0.99);
   cerr << (GetTime()-t) << "\n";
   mul(B1, U, B0);
   if (B1 != B) Error("bad LLLTest (1)");
   LLL(d, B, 90, 100);
   if (B1 != B) Error("bad LLLTest (2)");

   B = B0;
   cerr << "LLL...";
   t = GetTime();
   s = LLL(d, B, U);
   cerr << (GetTime()-t) << "\n";
   mul(B1, U, B0);
   if (B1 != B) Error("bad LLLTest (1)");

   cout << "rank = " << s << "\n";
   cout << "det = " << d << "\n";
   cout << "B = " << B << "\n";
   cout << "U = " << U << "\n";
}


// Copyright (c) 2016 Copyright Holder All Rights Reserved.

#include <algorithm>
#include <cstddef>
#include <cstdint>
#include <fstream>
#include <iostream>
#include <memory>
#include <numeric>

#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>

using namespace boost;

typedef adjacency_list<vecS, vecS, directedS, no_property,
                       property<edge_weight_t, int64_t>>
    Graph;

typedef graph_traits<Graph>::vertex_descriptor vertex_descriptor;
typedef graph_traits<Graph>::edge_descriptor edge_descriptor;

int main(int argc, char const *argv[]) {
  std::ifstream fin("../priv/edges.txt");
  int num_of_nodes, num_of_edges;

  fin >> num_of_nodes >> num_of_edges;
  std::cout << "Nodes " << num_of_nodes << " Edges " << num_of_edges
            << std::endl;
  Graph g(num_of_nodes);
  // std::vector<int64_t> weights(num_of_nodes);

  for (auto i = 0; i < num_of_edges; ++i) {
    int u, v;
    int64_t weight;
    fin >> u >> v >> weight;
    // std::cout << u << " -> " << v <<std::endl;
    add_edge(u, v, weight, g);
  }

  // this use dijskura, don't allow negative edge weights
  // std::vector<vertex_descriptor> p(num_vertices(g));
  // prim_minimum_spanning_tree(g, &p[0]);

  std::vector<edge_descriptor> spanning_tree;
  kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));

  for (auto it = spanning_tree.begin(); it != spanning_tree.end(); ++it) {
    std::cout << source(*it, g) << " -- " << target(*it, g) << ";" << std::endl;
    // std::cout << get(edge_weight, g, *it) << std::endl;
  }

  int64_t mst_weights = std::accumulate(
      spanning_tree.begin(), spanning_tree.end(), 0,
      [g](int64_t acc, auto b) { return acc + get(edge_weight, g, b); });

  std::cout << "MST weights: " << mst_weights << std::endl;

  /*for (std::size_t i = 0; i != p.size(); ++i)
    if (p[i] != i)
      std::cout << "parent[" << i << "] = " << p[i] << std::endl;
    else
      std::cout << "parent[" << i << "] = no parent" << std::endl;
*/
  // write_graphviz(std::cout, g);
  /*
    graph_traits<Graph>::edge_iterator ei, eend;
    for (boost::tie(ei, eend) = edges(g);  ei != eend; ++ei) {
      std::cout << "E: " << *ei << "   " << std::endl;
    }*/

  /*graph_traits<Graph>::vertex_iterator vi, vend;
  for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi) {
    std::cout << "V: " << *vi << std::endl;
  }*/

  // std::cout << num_edges(g) << std::endl;
  return 0;
}


#include <boost/test/unit_test.hpp>

#include "ternary_math.hpp"

BOOST_AUTO_TEST_SUITE(ternary_math)

BOOST_AUTO_TEST_CASE(abs_test)
{
    BOOST_TEST(abs_c(1) == 1);
    BOOST_TEST(abs_c(-1) == 1);
    BOOST_TEST(abs_c(0) == 0);
}

BOOST_AUTO_TEST_CASE(sign_test)
{
    BOOST_TEST(sign_c(42) == 1);
    BOOST_TEST(sign_c(-123) == -1);
    BOOST_TEST(sign_c(0) == 0);
}

BOOST_AUTO_TEST_CASE(power_of_3_test)
{
    BOOST_TEST(pow3(0) == 1);
    BOOST_TEST(pow3(1) == 3);
    BOOST_TEST(pow3(4) == 81);
}

BOOST_AUTO_TEST_CASE(lowest_trit_test)
{
    BOOST_TEST(lowest_trit(0) == 0);
    BOOST_TEST(lowest_trit(-1) == lowest_trit(2));
    BOOST_TEST(lowest_trit(31) == 1);
}

BOOST_AUTO_TEST_CASE(shift_right_test)
{
    auto tc1 { 42 };
    BOOST_TEST(shift_right(tc1, 0) == 42);
    BOOST_TEST(shift_right(tc1, 1) == 14);
    BOOST_TEST(shift_right(tc1, 2) == 5);
    BOOST_TEST(shift_right(tc1, 3) == 2);
    BOOST_TEST(shift_right(tc1, 4) == 1);
    BOOST_TEST(shift_right(tc1, 5) == 0);
}

BOOST_AUTO_TEST_CASE(nth_trit_test)
{
    auto tc2 { 52 };    // +-0-+
    BOOST_TEST(nth_trit(tc2, 0) == 1);
    BOOST_TEST(nth_trit(tc2, 1) == -1);
    BOOST_TEST(nth_trit(tc2, 2) == 0);
    BOOST_TEST(nth_trit(tc2, 3) == -1);
    BOOST_TEST(nth_trit(tc2, 4) == 1);
    BOOST_TEST(nth_trit(tc2, 5) == 0);
}

BOOST_AUTO_TEST_CASE(clamp_test)
{
    auto n1 { clamp<int,6>(0) };
    auto n2 { clamp<int,6>(123) };
    auto n3 { clamp<int,6>(-1234) };
    BOOST_TEST(n1 == 0);
    BOOST_TEST(n2 == 123);
    BOOST_TEST(n3 == -(pow3(6)/2));
}

BOOST_AUTO_TEST_CASE(low_trits_test)
{
    auto tc3 { 52 };    // +-0-+
    BOOST_TEST(low_trits(tc3, 0) == 0);
    BOOST_TEST(low_trits(tc3, 1) == 1);
    BOOST_TEST(low_trits(tc3, 2) == -2);
    BOOST_TEST(low_trits(tc3, 3) == -2);
    BOOST_TEST(low_trits(tc3, 4) == -29);
    BOOST_TEST(low_trits(tc3, 5) == 52);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <boost/container/static_vector.hpp>
#include <boost/container/vector.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <numeric>

namespace discreture
{

template <typename T>
void UNUSED(T&& /*unused*/)
{}

//////////////////////////////////////////
/// \brief This is what operator% should be but isn't (!).
///
/// C++ modulo operator%is dumb for negative integers: (-7)%3 returns -1,
/// instead of 2. This fixes it.
/// \return an integer in [0,b)
//////////////////////////////////////////
template <class IntType>
inline IntType modulo(IntType a, IntType b)
{
    IntType r = a%b;
    if (r < 0)
        r += b;
    return r;
}

//////////////////////////////////////////
/// \brief This is what operator %= should be but isn't (!).
///
/// C++ modulo operator %= is dumb for negative integers: (-7)%3 returns -1,
/// instead of 2. This fixes it.
//////////////////////////////////////////
template <class IntType>
inline void reduce_modulo(IntType& a, IntType b)
{
    a %= b;
    if (a < 0)
        a += b;
}

template <class T>
inline T pow(T a, std::uint64_t n)
{
    T r = 1;

    while (n > 0)
    {
        auto division = std::div(n, 2);
        if (division.rem == 1) // if odd
            r *= a;

        n = division.quot;
        a *= a;
    }

    return r;
}

// Deprecated in c++17. Here for use in c++14.
template <class T>
T gcd(T a, T b)
{
    while (b != 0)
    {
        T r = a%b;
        a = b;
        b = r;
    }
    return a;
}

template <class T, class Container>
T reduce_fraction(Container Numerator, Container Denominator)
{
    for (auto& b : Denominator)
    {
        for (auto& a : Numerator)
        {
            auto d = gcd(a, b);
            a /= d;
            b /= d;
            if (b == 1)
                break;
        }
    }

    T result = 1;
    for (auto a : Numerator)
        result *= a;
    return result;
}

} // namespace discreture


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

#include "src/liblm/c_api.h"
#include "src/liblm/liblm.h"

using namespace Eigen;

void test_c() {

    double *X = (double *) malloc(sizeof(double) * 5);
    double *y = (double *) malloc(sizeof(double) * 5);
    double *X_test = (double *) malloc(sizeof(double) * 5);
    for (int i = 0; i < 5; ++i) {
        X[i] = i;
        y[i] = 2 * i - 1;
        X_test[i] = i + 5;
    }
    lm_problem *prob = (lm_problem *) malloc(sizeof(lm_problem));
    dmatrix data;
    data.data = X;
    data.row = 5;
    data.col = 1;
    prob->X = data;
    prob->y = y;
    lm_param param;
    param.alg = REG;
    param.regu = NONE;
    lm_model *model = lm_train(prob, &param);
    dmatrix d;
    d.data = X_test;
    d.row = 5;
    d.col = 1;
    y = lm_predict(model, &d);
    for (int i = 0; i < 5; ++i)
        printf("%.3f ", y[i]);
    printf("\n");
    free_model(model);
}

void test_cpp() {
    MatrixXd X(4, 1);
    X << 1, 2, 3, 4;
    VectorXd y(4);
    y << 1, 2, 3, 4;
    MatrixXd X_test = X.array() + 5;
    Regressor *clf = new LinearRegression();
    clf->fit(X, y);
    std::cout << clf->predict(X_test) << std::endl;
    delete clf;
}

int main() {
    test_c();
    std::cout << "----------------" << std::endl;
    test_cpp();
    return 0;
}


#include <Eigen/Dense>
#include <vector>
#include <array>
#include <functional>
#include "plane3d.hpp"

using namespace Eigen;
using namespace std;

#ifndef MESH_COMPONENTS_HPP
#define MESH_COMPONENTS_HPP

class Vertex3d {
    Vector3d vec;
    //std::vector<Tetrahedron *> tetrahedrons;
  public:
    Vertex3d (array<double, 3> _vec);
    Vector3d Vec();
};

class Tetrahedron {
    unsigned long int id;
    vector<reference_wrapper<Vertex3d>> vertices;
    vector<unsigned long int> neighbors;
    array<double, 3> sphereCenter;
    double sphereRadius;
    double weight;
    int label;
  public:
    Tetrahedron (const int id, const vector<reference_wrapper<Vertex3d>> _vertices, const double _weight, const int _label);
    void addNeighbor(unsigned long int neighborId);
    bool contains(const array<double, 3>);
    vector<reference_wrapper<Vertex3d>> Vertices();
    vector<unsigned long int> Neighbors();
    vector<array<double, 3>> intersectsPlane(Plane3d plane);
    unsigned long int Id();
    double Weight();
    int Label();
};

class Face {
    vector<reference_wrapper<Vertex3d>> vertices;
    unsigned long int tetId;
  public:
    Face (vector<reference_wrapper<Vertex3d>> _vertices, unsigned long int _tetId);
    vector<reference_wrapper<Vertex3d>> Vertices();
    unsigned long int TetId();
    bool intersectsPlane(Plane3d plane);
    //vector<Vertex3d *> Vertices();
    //Tetrahedron* getTetrahedron();
};

#endif


//=======================================================================
// 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 <vector>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/bellman_ford_shortest_paths.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;
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  // VC++ can't handle the iterator constructor
  Graph g(N);
  for (std::size_t j = 0; j < n_edges; ++j)
    add_edge(edge_array[j].first, edge_array[j].second, g);
#else
  Graph g(edge_array, edge_array + n_edges, N);
#endif
  graph_traits < Graph >::edge_iterator ei, ei_end;
  property_map<Graph, int EdgeProperties::*>::type
    weight_pmap = get(&EdgeProperties::weight, g);
  int i = 0;
  for (boost::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;

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  bool r = bellman_ford_shortest_paths
    (g, int(N), weight_pmap, &parent[0], &distance[0],
     closed_plus<int>(), std::less<int>(), default_bellman_visitor());
#else
  bool r = bellman_ford_shortest_paths
    (g, int (N), weight_map(weight_pmap).distance_map(&distance[0]).
     predecessor_map(&parent[0]));
#endif

  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::ofstream dot_file("figs/bellman-eg.dot");
  dot_file << "digraph D {\n"
    << "  rankdir=LR\n"
    << "  size=\"5,3\"\n"
    << "  ratio=\"fill\"\n"
    << "  edge[style=\"bold\"]\n" << "  node[shape=\"circle\"]\n";

  {
    for (boost::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 << "]";
    }
  }
  dot_file << "}";
  return EXIT_SUCCESS;
}


#include <iostream>
#include <unordered_map>
#include <string>
#include <vector>
#include <boost/functional/hash.hpp>

using namespace std;

#define DEBUG 0

#define MAX_W 100
#define MAX_L 100

typedef tuple<int, int, int, int> quad;
template<> struct hash<quad> {
  size_t operator()(const quad& q) const noexcept {
    size_t seed = 0;
    boost::hash_combine(seed, get<0>(q));
    boost::hash_combine(seed, get<1>(q));
    boost::hash_combine(seed, get<2>(q));
    boost::hash_combine(seed, get<3>(q));
    return seed;
  }
};

class AvoidRoads {
private:
  long dp[MAX_W+1][MAX_L+1] = { 0 };
  unordered_map<quad, int>bad_m;

public:
  long numWays(int w, int l, vector<string> bad) {
    preparebad(bad);

    dp[0][0] = 1;

    // Initialize bottom row
    for (int i = 1; i < w + 1; ++i) {
      quad p = make_tuple(i - 1, 0, i, 0);
      if (bad_m.find(p) == bad_m.end()) {
        dp[i][0] = dp[i-1][0];
      }
      int j = 0;
      // cout << "i: " << i << ", j: " << j << ", dp[i][j]: " << dp[i][j] << endl;
    }
    // Initialize left column
    for (int j = 1; j < l + 1; ++j) {
      quad p = make_tuple(0, j - 1, 0, j);
      if (bad_m.find(p) == bad_m.end()) {
        dp[0][j] = dp[0][j-1];
      }
      int i = 0;
      // cout << "i: " << i << ", j: " << j << ", dp[i][j]: " << dp[i][j] << endl;
    }
    for (int i = 1; i < w + 1; ++i) {
      for (int j = 1; j < l + 1; ++j) {
        if (bad_m.find(make_tuple(i - 1, j, i, j)) == bad_m.end()) {
          dp[i][j] += dp[i-1][j];
        }
        if (bad_m.find(make_tuple(i, j - 1, i, j)) == bad_m.end()) {
          dp[i][j] += dp[i][j-1];
        }
        // cout << "i: " << i << ", j: " << j << ", dp[i][j]: " << dp[i][j] << endl;
      }
    }

    // Only for debug
    // printf("\n");
    // for (int j = l; j >= 0; --j) {
    //   for (int i = 0; i < w + 1; ++i) {
    //     printf("%5ld", dp[i][j]);
    //   }
    //   printf("\n");
    // }
    return dp[w][l];
  }

  void preparebad(vector<string> bad) {
    for(auto &s : bad) {
      // NOTE: read int from bad string
      int l = s.size();
      vector<int> space;
      for (int i = 0; i < l; ++i) {
        if (s[i] == ' ') {
          space.push_back(i);
        }
      }

      vector<int> pos_list;
      pos_list.push_back(stoi(s));
      for(auto &p : space) {
        pos_list.push_back(stoi(s.substr(p + 1)));
      }

      quad p;
      if (pos_list[0] < pos_list[2] || pos_list[1] < pos_list[3]) {
        p = make_tuple(pos_list[0], pos_list[1], pos_list[2], pos_list[3]);
      } else {
        p = make_tuple(pos_list[2], pos_list[3], pos_list[0], pos_list[1]);
      }
      bad_m[p] = 1;
    }
  }
};

int main(int argc, char** argv) {
  AvoidRoads a0;
  vector<string> bad0 = {"0 0 0 1", "6 6 5 6"};
  std::cout << "a0: Expected 252, Got " << a0.numWays(6, 6, bad0) << "" << std::endl;

  AvoidRoads a1;
  vector<string> bad1 = {};
  std::cout << "a1: Expected 2, Got " << a1.numWays(1, 1, bad1) << "" << std::endl;

  AvoidRoads a2;
  vector<string> bad2 = {};
  std::cout << "a1: Expected 6406484391866534976, Got " << a2.numWays(35, 31, bad2) << "" << std::endl;

  AvoidRoads a3;
  vector<string> bad3 = {"0 0 1 0", "1 2 2 2", "1 1 2 1"};
  std::cout << "a1: Expected 0, Got " << a3.numWays(2, 2, bad3) << "" << std::endl;

  return 0;
}


#include "Normals.h"

#include <Eigen/Geometry>

void perVertexNormals(const Eigen::MatrixXd &vertices,
	const Eigen::MatrixXi &triangles,
	Eigen::MatrixXd &normals,
	WeightType::Type weightType)
{
	normals.setZero(vertices.rows(), vertices.cols());
	std::vector<double> weightSum(vertices.cols(), 0.0);

	for (int tri = 0; tri < triangles.cols(); tri++)
	{
		std::array<int, 3> vIdcs = { triangles(0, tri), triangles(1, tri), triangles(2, tri) };
		std::array<Eigen::Vector3d, 3> v;
		for (int i = 0; i < 3; i++) {
			if (vIdcs[i] < 0 || vIdcs[i] >= vertices.cols()) {
				throw std::logic_error("err");
			}
			v[i] = vertices.col(vIdcs[i]);
		}
		Eigen::Vector3d normal = (v[1] - v[0]).cross(v[2] - v[0]);
		double twiceArea = normal.norm();
		normal /= twiceArea;
		for (int i = 0; i < 3; i++)
		{
			Eigen::Vector3d e1 = v[(i + 1) % 3] - v[i];
			Eigen::Vector3d e2 = v[(i + 2) % 3] - v[i];
			double angleBetween = std::acos(std::min(1.0, std::max(-1.0, e1.normalized().dot(e2.normalized()))));
			double weight;
			switch (weightType)
			{
			case WeightType::CONSTANT:
				weight = 1.0;
				break;
			case WeightType::AREA:
				weight = twiceArea;
				break;
			case WeightType::ANGLE:
				weight = angleBetween;
				break;
			case WeightType::ANGLE_AREA:
				weight = twiceArea * angleBetween;
				break;
			default: assert(false);
			}
			//double weight = 1.0;
			Eigen::Vector3d wn = weight*normal;
			normals.col(vIdcs[i]) += wn;
			weightSum[vIdcs[i]] += weight;
		}
	}

	for (int i = 0; i < vertices.cols(); i++)
	{
		if (weightSum[i] != 0.0)
		{
			normals.col(i) /= weightSum[i];
		}
	}
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix4f A = MatrixXf::Random(4,4);
cout << "Here is a random 4x4 matrix:" << endl << A << endl;
HessenbergDecomposition<MatrixXf> hessOfA(A);
MatrixXf H = hessOfA.matrixH();
cout << "The Hessenberg matrix H is:" << endl << H << endl;
MatrixXf Q = hessOfA.matrixQ();
cout << "The orthogonal matrix Q is:" << endl << Q << endl;
cout << "Q H Q^T is:" << endl << Q * H * Q.transpose() << endl;

  return 0;
}


#include "tests_stats.hxx"

#include "gtest/gtest.h"
#include "vec3f.hxx"
#include "util.hxx"

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/numeric/interval.hpp>

#include <numeric> // for std::accumulate1

// A review of statistical testing applied to computer graphics!
// http://www0.cs.ucl.ac.uk/staff/K.Subr/Files/Papers/PG2007.pdf

//http://mathworld.wolfram.com/BinomialDistribution.html
std::tuple<double, double> BinomialDistributionMeanStdev(int n, double p)
{
  assert(p >= 0.);
  return std::make_tuple(p*n, std::sqrt(n*p*(1.-p)));
}

std::tuple<double, double> BinomialDistributionMeanStdev(int _n, double _p, double _p_err)
{
  assert(_p >= 0. && _p_err >= 0.);
  using namespace boost::numeric;
  using namespace interval_lib;
  using I = interval<double>;
  double pl = std::max(0., _p-_p_err);
  double ph = std::min(1., _p+_p_err);
  I p{pl, ph};
  I n{_n, _n};
  I mean = p*n;
  I stddev = sqrt(n*p*(1.-p));
  double uncertainty = stddev.upper() + 0.5*(mean.upper()-mean.lower());
  return std::make_tuple(_p*_n, uncertainty);
}

void CheckNumberOfSamplesInBin(const char *name, int num_smpl_in_bin, int total_num_smpl, double p_of_bin, double number_of_sigmas_threshold, double p_of_bin_error)
{
  double mean, sigma;
  std::tie(mean, sigma) = BinomialDistributionMeanStdev(total_num_smpl, p_of_bin, p_of_bin_error);
  std::stringstream additional_output;
  if (name)
    additional_output << "Expected in " << name << ": " << mean << "+/-" << sigma << " Actual: " << num_smpl_in_bin << " of " << total_num_smpl << std::endl;
  EXPECT_NEAR(num_smpl_in_bin, mean, sigma*number_of_sigmas_threshold) << additional_output.str();
}

namespace ChiSqrInternal
{

struct Bin {
  double count;
  double expected;
};

// TODO: I wish I had a super lightweight range checked (in debug mode only ofc.) array view.

void MergeBinsToGetAboveMinNumSamplesCutoff(Bin *bins, int &num_bins, double low_expected_num_samples_cutoff)
{
  //http://en.cppreference.com/w/cpp/algorithm/make_heap
  // ... and so on.
  auto cmp = [](const Bin &a, const Bin &b) -> bool { return a.expected > b.expected; }; // Using > instead of < makes it a min-heap.
  std::make_heap(bins, bins+num_bins, cmp);
  while (num_bins > 1)
  {
    // Get smallest element to the end.
    std::pop_heap(bins, bins+num_bins, cmp);
    if (bins[num_bins-1].expected > low_expected_num_samples_cutoff)
      break;
    // Get the second smallest element to the second last position.
    std::pop_heap(bins, bins+num_bins-1, cmp);
    // Merge them.
    Bin &merged = bins[num_bins-2];
    Bin &removed = bins[num_bins-1];
    merged.count += removed.count;
    merged.expected += removed.expected;
    removed.count = 0;
    removed.expected = 0;
    --num_bins;
    // Modifications done. Get last element back into heap.
    std::push_heap(bins, bins+num_bins, cmp);
  }
  assert(num_bins >= 1);
  assert((num_bins == 1) || (bins[num_bins-1].expected > low_expected_num_samples_cutoff));
}

}

double ChiSquaredProbability(const int *counts, const double *weights, int num_bins, double low_expected_num_samples_cutoff)
{
  using namespace ChiSqrInternal;

  int num_samples = std::accumulate(counts, counts+num_bins, 0); // It's the sum.
  double probability_normalization = std::accumulate(weights, weights+num_bins, 0.);

  probability_normalization = probability_normalization>0. ? 1./probability_normalization : 0.;

  std::unique_ptr<Bin[]> bins{new Bin[num_bins]};
  for (int i=0; i<num_bins; ++i)
    bins[i] = Bin{(double)counts[i], weights[i]*num_samples*probability_normalization};

  int original_num_bins = num_bins;
  MergeBinsToGetAboveMinNumSamplesCutoff(bins.get(), num_bins, low_expected_num_samples_cutoff);

  //std::cout << "Chi Sqr bins: " << num_bins << std::endl;
  double chi_sqr = 0.;
  for (int i=0; i<num_bins; ++i)
  {
    //std::cout << strconcat(i, ": e=", bins[i].expected, ", cnt=",bins[i].count) << std::endl;
    chi_sqr += Sqr(bins[i].count - bins[i].expected)/bins[i].expected;
  }

  if (original_num_bins > num_bins*3)
  {
    std::cout << "Chi-Sqr Test merged " << (original_num_bins-num_bins) << " of " << original_num_bins << " bins because their sample count was below " << low_expected_num_samples_cutoff << std::endl;
  }

  // https://www.boost.org/doc/libs/1_66_0/libs/math/doc/html/math_toolkit/dist_ref/dists/chi_squared_dist.html
  // https://www.boost.org/doc/libs/1_66_0/libs/math/doc/html/math_toolkit/dist_ref/nmp.html#math_toolkit.dist_ref.nmp.cdf
  boost::math::chi_squared_distribution<double> distribution(num_bins-1);
  double prob_observe_ge_chi_sqr = cdf(complement(distribution, chi_sqr));

  //std::cout << "chi-sqr prob = " << prob_observe_ge_chi_sqr << std::endl;
  //EXPECT_GE(prob_observe_ge_chi_sqr, p_threshold);
  return prob_observe_ge_chi_sqr;
}

double StddevOfAverage(double sample_stddev, int num_samples)
{
  return sample_stddev / std::sqrt(num_samples);
}

double StddevOfStddev(double sample_stddev, int num_samples)
{
  return std::sqrt(2. / (num_samples - 1))*Sqr(sample_stddev);
}

double StudentsTValue(double sample_avg, double avg_stddev, double true_mean)
{
  return (sample_avg - true_mean) / avg_stddev;
}

double StudentsTDistributionOutlierProbability(double t, double num_samples)
{
  boost::math::students_t_distribution<double> distribution(num_samples);
  if (t > 0.)
    return cdf(complement(distribution, t));
  else
    return cdf(distribution, t);
}

void TestSampleAverage(double sample_avg, double sample_stddev, double num_samples, double true_mean, double true_mean_error, double p_value)
{
  double avg_stddev = StddevOfAverage(sample_stddev, num_samples);
  if (avg_stddev > 0.)
  {
    double prob;
    // Normally I would use the probability of the distance greater than |(sample_avg - true_mean)| w.r.t. to a normal distribution.
    // I use normal distribution rather than t-test stuff because I have a lot of samples.
    // However, since I also have integration errors, I "split" the normal distribution in half and move both sides to the edges of
    // the integration error bounds. Clearly, I the MC estimate is very good, I want to check if sample_avg is precisely within the
    // integration bounds. On the other hand, if the integration error is zero, I want to use the usual normal distribution as
    // stated above. My scheme here achives both of these edge cases.
    if (sample_avg < true_mean - true_mean_error)
      prob = StudentsTDistributionOutlierProbability(StudentsTValue(sample_avg, avg_stddev, true_mean - true_mean_error), num_samples);
    else if(sample_avg > true_mean + true_mean_error)
      prob = StudentsTDistributionOutlierProbability(StudentsTValue(sample_avg, avg_stddev, true_mean + true_mean_error), num_samples);
    else
      prob = StudentsTDistributionOutlierProbability(0., num_samples);
    EXPECT_GE(prob, p_value) << "Probability to find sample average " << sample_avg << " +/- " << avg_stddev << " w.r.t. true mean " << true_mean << "+/-" << true_mean_error <<
    " is " << prob << " which is lower than p-value " << p_value << std::endl;
    //std::cout << "Test Sample Avg Prob = " << prob << std::endl;
  }
  else
  {
    EXPECT_NEAR(sample_avg, true_mean, true_mean_error);
  }
}

void TestProbabilityOfMeanLowerThanUpperBound(double sample_avg, double sample_stddev, double num_samples, double bound, double p_value)
{
  double avg_stddev = StddevOfAverage(sample_stddev, num_samples);
  if (avg_stddev > 0.)
  {
    boost::math::students_t_distribution<double> distribution(num_samples);
    double prob = cdf(complement(distribution, StudentsTValue(sample_avg, avg_stddev, bound)));
    EXPECT_GE(prob, p_value) << "Probability to find the mean smaller than " << bound << " is " << prob << " which is lower than the p-value " << p_value << ", given the average " << sample_avg << " +/- " << avg_stddev << std::endl;
    //std::cout << "Test Prob Upper Bound = " << prob << std::endl;
  }
  else
  {
    EXPECT_LE(sample_avg, bound);
  }
}

TEST(BinomialDistribution, WithIntervals)
{
  double mean1, stddev1;
  double mean2, stddev2;
  std::tie(mean1, stddev1) = BinomialDistributionMeanStdev(10, 0.6);
  std::tie(mean2, stddev2) = BinomialDistributionMeanStdev(10, 0.6, 0.);
  EXPECT_NEAR(mean1, mean2, 1.e-9);
  EXPECT_NEAR(stddev1, stddev2, 1.e-9);
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <iostream>

#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/functional/operator.hpp>

namespace compute = boost::compute;

// this example demonstrates how to use Boost.Compute's STL
// implementation to add two vectors on the GPU
int main()
{
    // setup input arrays
    float a[] = { 1, 2, 3, 4 };
    float b[] = { 5, 6, 7, 8 };

    // make space for the output
    float c[] = { 0, 0, 0, 0 };

    // create vectors and transfer data for the input arrays 'a' and 'b'
    compute::vector<float> vector_a(a, a + 4);
    compute::vector<float> vector_b(b, b + 4);

    // create vector for the output array
    compute::vector<float> vector_c(4);

    // add the vectors together
    compute::transform(
        vector_a.begin(),
        vector_a.end(),
        vector_b.begin(),
        vector_c.begin(),
        compute::plus<float>()
    );

    // transfer results back to the host array 'c'
    compute::copy(vector_c.begin(), vector_c.end(), c);

    // print out results in 'c'
    std::cout << "c: [" << c[0] << ", "
                        << c[1] << ", "
                        << c[2] << ", "
                        << c[3] << "]" << std::endl;

    return 0;
}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-ieee
    Function object implementing ldexp capabilities

    The function multiply a floating entry \f$x\f$
    by \f$2^{n}\f$

    @par Semantic:

    @code
    auto r = ldexp(x,n);
    @endcode

    is similar to:

    @code
    auto r = x*pow(2, trunc(n));
    @endcode

    @par Decorators

    By default @c ldexp does not take care of denormal or limiting values. Use the @c pedantic_ decorator if these are
    to be properly computed.

    The @ref cardinal_of and the size of elements value of the types of @c x and @c n must be identical

  **/
  Value ldexp(Value const & x, Value const& n);
} }
#endif

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

#endif


#include "Sphere.h"
#include "Ray.h"
#include "Intersection.h"

#include <cmath>

#include <Eigen/Dense>

std::unique_ptr<Intersection> Sphere::intersect(const Ray& ray) {
    auto p = ray.p - center;
    auto d = ray.d;

    float dp = d.dot(p);
    float dd = d.dot(d);
    float pp = p.dot(p);

    float discr = (dp*dp) - (dd*(pp-(radius*radius)));

    if (discr < 0) {
        return nullptr;
    } else {
        // Find both intersections
        float t0 = (-dp - sqrt(discr)) / dd;
        float t1 = (-dp + sqrt(discr)) / dd;

        // Entire sphere is behind camera
        if (t1 < 0)
            return nullptr;

        // Front of sphere is behind camera
        float t = t0 < 0 ? t1 : t0;

        // Calculate surface normal
        auto n = (ray.evaluate(t) - center).normalized();

        return std::unique_ptr<Intersection>(new Intersection(this, n, t));
    }
}


#ifndef BOOST_UBLAS_DAVIDON_FLETCHER_POWELL_HPP
#define BOOST_UBLAS_DAVIDON_FLETCHER_POWELL_HPP

#include <cfloat> // DBL_EPSILON
#include <boost/assert.hpp>
#include "function.hpp"

namespace boost {
    namespace numeric {
        namespace ublas {
            namespace details {

                template <class T>
                T find_min(const Function<T> & f, const vector<T> & x, vector<T> const & d, T a, T b, const double eps = 1E-8) {
                    T e = 0;
                    T x_1 = T(0);
                    T x_2 = T(0);

                    while (std::abs(b - a) >= eps) {
                        e = (b - a) * 1E-2 * 1E-3;
                        x_1 = (b + a) / 2 - e;
                        x_2 = (b + a) / 2 + e;

                        if (f(x + x_1 * d) > f(x + x_2 * d)) {
                            a = x_1;
                        }
                        else {
                            b = x_2;
                        }
                    }
                    return (a + b) / 2;
                }
            } /* namespace details */

            template <class T>
            vector<T> DavidonFletcherPowell(const Function<T> & f, vector<T> x, const double epsilon, std::size_t & number_iteration) {
                BOOST_ASSERT(f.function);
                BOOST_ASSERT(f.gradient);

                vector<T> grad(f.gradient(x));
                matrix<T> eta(identity_matrix<T>(f.size));

                vector<T> old_x = x;
                vector<T> old_grad = grad;

                vector<T> d;
                T alpha = T(0);

                vector<T> delta_x;
                vector<T> delta_g;
                matrix<T> A;
                matrix<T> B;

                for (; ublas::norm_2(grad) >= epsilon && number_iteration < 1E6; old_x = x, old_grad = grad, ++number_iteration) {
                    d = -prod(eta, grad);

                    // Looking for the minimum of F(x - alpha * d) using the method of one-dimensional optimization.
                    alpha = details::find_min(f, x, d, T(0.0), T(1E3));
                    BOOST_ASSERT(alpha >= DBL_EPSILON);

                    x = x + alpha * d;
                    grad = f.gradient(x);

                    delta_x = x - old_x;
                    delta_g = grad - old_grad;

                    A = (outer_prod(delta_x, delta_x) / inner_prod(delta_x, delta_g));

                    B = prod(outer_prod(static_cast<vector<T>>(prod(eta, delta_g)), delta_g), trans(eta))
                        / inner_prod(static_cast<vector<T>>(prod(static_cast<vector<T>>(prod(identity_matrix<T>(f.size), delta_g)), eta)), delta_g);

                    eta = eta + A - B;
                }

                return x;
            }
        } /* namespace ublas */
    } /* namespace numeric */
} /* namespace boost */
#endif


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.

#include <vector>
#include <unordered_map>
#include <string>

#include <Eigen/Dense>
#include <Eigen/StdVector>

#include "hand_eigen_d.h"
#include "../../shared/defs.h"
#include "../../shared/utils.h"
#include "../../shared/hand_eigen_model.h"

using std::vector;
using std::unordered_map;
using std::string;
using Eigen::Map;
using Eigen::Vector3d;
using Eigen::Matrix3Xd;
using Eigen::Matrix4d;
using Eigen::aligned_allocator;
using Eigen::AngleAxisd;
using Eigen::Matrix3d;
using Eigen::MatrixXd;

void angle_axis_to_rotation_matrix_d(
    const Vector3d& angle_axis,
    Matrix3d* R,
    vector<Matrix3d>* pdR)
{
    auto& dR = *pdR;
    double sqnorm = angle_axis.squaredNorm();
    double norm = sqrt(sqnorm);
    if (norm < .0001)
    {
        R->setIdentity();
        for (int i = 0; i < 3; i++)
            dR[i].setZero();
        return;
    }
    double inv_norm = 1. / norm;
    double inv_sqnorm = 1. / sqnorm;
    Vector3d d_norm = angle_axis * inv_norm;

    double x = angle_axis(0) * inv_norm;
    double y = angle_axis(1) * inv_norm;
    double z = angle_axis(2) * inv_norm;
    Vector3d dx, dy, dz;
    for (int i = 0; i < 3; i++)
    {
        dx(i) = -angle_axis(0) * d_norm(i) * inv_sqnorm;
        dy(i) = -angle_axis(1) * d_norm(i) * inv_sqnorm;
        dz(i) = -angle_axis(2) * d_norm(i) * inv_sqnorm;
    }
    dx(0) += norm * inv_sqnorm;
    dy(1) += norm * inv_sqnorm;
    dz(2) += norm * inv_sqnorm;

    double s = sin(norm);
    double c = cos(norm);
    Vector3d dc, ds;
    ds = c * d_norm;
    dc = -s * d_norm;

    *R << x * x + (1 - x * x) * c, x* y* (1 - c) - z * s, x* z* (1 - c) + y * s,
        x* y* (1 - c) + z * s, y* y + (1 - y * y) * c, y* z* (1 - c) - x * s,
        x* z* (1 - c) - y * s, z* y* (1 - c) + x * s, z* z + (1 - z * z) * c;

    for (int i = 0; i < 3; i++)
    {
        dR[i] << 2 * x * dx(i) - 2 * x * dx(i) * c + (1 - x * x) * dc(i),
            dx(i) * y * (1 - c) + x * dy(i) * (1 - c) - x * y * dc(i) - dz(i) * s - z * ds(i),
            dx(i) * z * (1 - c) + x * dz(i) * (1 - c) - x * z * dc(i) + dy(i) * s + y * ds(i),
            dx(i)* y* (1 - c) + x * dy(i) * (1 - c) - x * y * dc(i) + dz(i) * s + z * ds(i),
            2 * y * dy(i) - 2 * y * dy(i) * c + (1 - y * y) * dc(i),
            dy(i) * z * (1 - c) + y * dz(i) * (1 - c) - y * z * dc(i) - dx(i) * s - x * ds(i),
            dx(i)* z* (1 - c) + x * dz(i) * (1 - c) - x * z * dc(i) - dy(i) * s - y * ds(i),
            dz(i)* y* (1 - c) + z * dy(i) * (1 - c) - z * y * dc(i) + dx(i) * s + x * ds(i),
            2 * z * dz(i) - 2 * z * dz(i) * c + (1 - z * z) * dc(i);
    }
}

void apply_global_transform_d(
    const vector<int>& corresp,
    const Matrix3Xd& pose_params,
    Matrix3Xd* ppositions,
    double* pJ,
    Matrix3d* pR)
{
    auto& R = *pR;
    auto& positions = *ppositions;
    vector<Matrix3d> dR(3);
    const Vector3d& global_rotation = pose_params.col(0);
    angle_axis_to_rotation_matrix_d(global_rotation, &R, &dR);
    R.noalias() = (R.array().rowwise() * pose_params.col(1).transpose().array()).matrix();
    for (int i = 0; i < 3; i++)
        dR[i].noalias() = (dR[i].array().rowwise() * pose_params.col(1).transpose().array()).matrix();

    // global rotation
    size_t npts = corresp.size();
    for (int i_param = 0; i_param < 3; i_param++)
    {
        Map<Matrix3Xd> J_glob_rot(&pJ[i_param * 3 * npts], 3, npts);
        for (size_t i_pt = 0; i_pt < npts; i_pt++)
        {
            J_glob_rot.col(i_pt).noalias() = -dR[i_param] * positions.col(corresp[i_pt]);
        }
    }

    // global translation
    Map<MatrixXd> J_glob_translation(&pJ[3 * 3 * npts], 3 * npts, 3);
    for (size_t i = 0; i < npts; i++)
    {
        J_glob_translation.middleRows(i * 3, 3).setIdentity();
    }
    J_glob_translation *= -1.;

    positions = (R * positions).colwise() + pose_params.col(2);
}

void apply_global_transform_d(
    const double* const us,
    const vector<Triangle>& triangles,
    const vector<int>& corresp,
    const Matrix3Xd& pose_params,
    Matrix3Xd* ppositions,
    double* pJ,
    Matrix3d* pR)
{
    auto& R = *pR;
    auto& positions = *ppositions;
    vector<Matrix3d> dR(3);
    const Vector3d& global_rotation = pose_params.col(0);
    angle_axis_to_rotation_matrix_d(global_rotation, &R, &dR);
    R.noalias() = (R.array().rowwise() * pose_params.col(1).transpose().array()).matrix();
    for (int i = 0; i < 3; i++)
        dR[i].noalias() = (dR[i].array().rowwise() * pose_params.col(1).transpose().array()).matrix();

    // global rotation
    size_t npts = corresp.size();
    for (int i_param = 0; i_param < 3; i_param++)
    {
        Map<Matrix3Xd> J_glob_rot(&pJ[i_param * 3 * npts], 3, npts);
        for (size_t i_pt = 0; i_pt < npts; i_pt++)
        {
            const auto& verts = triangles[corresp[i_pt]].verts;
            const double* const u = &us[2 * i_pt];

            Vector3d tmp = u[0] * positions.col(verts[0]) + u[1] * positions.col(verts[1])
                + (1. - u[0] - u[1]) * positions.col(verts[2]);

            J_glob_rot.col(i_pt).noalias() = -dR[i_param] * tmp;
        }
    }

    // global translation
    Map<MatrixXd> J_glob_translation(&pJ[3 * 3 * npts], 3 * npts, 3);
    for (size_t i = 0; i < npts; i++)
    {
        J_glob_translation.middleRows(i * 3, 3).setIdentity();
    }
    J_glob_translation *= -1.;

    positions = (R * positions).colwise() + pose_params.col(2);
}

void relatives_to_absolutes_d(
    const avector<Matrix4d>& relatives,
    const avector<Matrix4d>& relatives_d,
    const vector<int>& parents,
    avector<Matrix4d>* pabsolutes,
    vector<avector<Matrix4d>>* pabsolutes_d)
{
    auto& absolutes = *pabsolutes;
    auto& absolutes_d = *pabsolutes_d;
    absolutes.resize(parents.size());
    absolutes_d.resize(parents.size());
    int rel_d_tail = 0;
    for (size_t i = 0; i < parents.size(); i++)
    {
        if (parents[i] == -1)
            absolutes[i].noalias() = relatives[i];
        else
            absolutes[i].noalias() = absolutes[parents[i]] * relatives[i];

        int n_finger_bone = (i - 1) % 4;
        if (i > 0 && i < parents.size() - 1 && n_finger_bone > 0)
        {
            absolutes_d[i].resize(n_finger_bone + 1);
            int curr_tail = 0;

            for (const auto& absolute_d_parent : absolutes_d[parents[i]])
                absolutes_d[i][curr_tail++].noalias() = absolute_d_parent * relatives[i];

            absolutes_d[i][curr_tail++].noalias() = absolutes[parents[i]] * relatives_d[rel_d_tail++];
            if (n_finger_bone == 1)
                absolutes_d[i][curr_tail++].noalias() = absolutes[parents[i]] * relatives_d[rel_d_tail++];
        }
    }
}

void euler_angles_to_rotation_matrix(
    const Vector3d& xzy,
    Matrix3d* pR,
    Matrix3d* pdR0,
    Matrix3d* pdR1)
{
    double tx = xzy(0), ty = xzy(2), tz = xzy(1);
    Matrix3d Rx = Matrix3d::Identity(),
        Ry = Matrix3d::Identity(),
        Rz = Matrix3d::Identity();
    Rx(1, 1) = cos(tx);
    Rx(2, 1) = sin(tx);
    Rx(1, 2) = -Rx(2, 1);
    Rx(2, 2) = Rx(1, 1);

    Ry(0, 0) = cos(ty);
    Ry(0, 2) = sin(ty);
    Ry(2, 0) = -Ry(0, 2);
    Ry(2, 2) = Ry(0, 0);

    Rz(0, 0) = cos(tz);
    Rz(1, 0) = sin(tz);
    Rz(0, 1) = -Rz(1, 0);
    Rz(1, 1) = Rz(0, 0);

    Matrix3d RzRy = Rz * Ry;
    if (pdR0)
    {
        Matrix3d dRx = Matrix3d::Zero();
        dRx(1, 1) = -Rx(2, 1);
        dRx(2, 1) = Rx(1, 1);
        dRx(1, 2) = -dRx(2, 1);
        dRx(2, 2) = dRx(1, 1);
        pdR0->noalias() = RzRy * dRx;
    }
    if (pdR1)
    {
        Matrix3d dRz = Matrix3d::Zero();
        dRz(0, 0) = -Rz(1, 0);
        dRz(1, 0) = Rz(0, 0);
        dRz(0, 1) = -dRz(1, 0);
        dRz(1, 1) = dRz(0, 0);
        pdR1->noalias() = dRz * Ry * Rx;
    }

    pR->noalias() = RzRy * Rx;
}

void get_posed_relatives_d(
    const HandModelEigen& model,
    const Matrix3Xd& pose_params,
    avector<Matrix4d>* prelatives,
    avector<Matrix4d>* prelatives_d)
{
    auto& relatives = *prelatives;
    auto& relatives_d = *prelatives_d;
    relatives.resize(model.base_relatives.size());
    relatives_d.resize(4 * 5); // 4 parameters in every finger

    int offset = 3;
    int tail = 0;
    for (size_t i = 0; i < model.bone_names.size(); i++)
    {
        Matrix4d tr = Matrix4d::Identity();

        Matrix3d R;
        int n_finger_bone = (i - 1) % 4;
        if (i == 0 || i == model.bone_names.size() - 1 || n_finger_bone == 0)
        {
            euler_angles_to_rotation_matrix(pose_params.col(i + offset), &R);
        }
        else
        {
            Matrix4d dtr0 = Matrix4d::Zero();
            Matrix3d dR0;
            if (n_finger_bone == 1)
            {
                Matrix4d dtr1 = Matrix4d::Zero();
                Matrix3d dR1;
                euler_angles_to_rotation_matrix(pose_params.col(i + offset), &R, &dR0, &dR1);
                dtr1.block(0, 0, 3, 3) = dR1;
                relatives_d[tail + 1].noalias() = model.base_relatives[i] * dtr1;
            }
            else
                euler_angles_to_rotation_matrix(pose_params.col(i + offset), &R, &dR0);
            dtr0.block(0, 0, 3, 3) = dR0;
            relatives_d[tail++].noalias() = model.base_relatives[i] * dtr0;
            if (n_finger_bone == 1)
                tail++;
        }
        tr.block(0, 0, 3, 3) = R;

        relatives[i].noalias() = model.base_relatives[i] * tr;
    }
}

void get_skinned_vertex_positions_d_common(
    const HandModelEigen& model,
    const Matrix3Xd& pose_params,
    const vector<int>& corresp,
    Matrix3Xd* positions,
    vector<Matrix3Xd>* positions_d,
    double* pJ,
    bool apply_global)
{
    avector<Matrix4d> relatives, absolutes, transforms;
    avector<Matrix4d> relatives_d;
    vector<avector<Matrix4d>> absolutes_d, transforms_d;
    get_posed_relatives_d(model, pose_params, &relatives, &relatives_d);
    relatives_to_absolutes_d(relatives, relatives_d, model.parents, &absolutes, &absolutes_d);

    // Get bone transforms.
    transforms.resize(absolutes.size());
    transforms_d.resize(absolutes.size());
    for (size_t i = 0; i < absolutes.size(); i++)
    {
        transforms[i].noalias() = absolutes[i] * model.inverse_base_absolutes[i];
        transforms_d[i].resize(absolutes_d[i].size());
        for (size_t j = 0; j < absolutes_d[i].size(); j++)
            transforms_d[i][j].noalias() = absolutes_d[i][j] * model.inverse_base_absolutes[i];
    }

    // Transform vertices by necessary transforms. + apply skinning
    *positions = Matrix3Xd::Zero(3, model.base_positions.cols());
    positions_d->resize(4 * 5, Matrix3Xd::Zero(3, model.base_positions.cols()));
    for (int i = 0; i < (int)transforms.size(); i++)
    {
        *positions +=
            ((transforms[i] * model.base_positions.colwise().homogeneous()).array()
                .rowwise() * model.weights.row(i)).matrix()
            .topRows(3);

        int i_finger = (i - 1) / 4;
        for (int j = 0; j < (int)transforms_d[i].size(); j++)
        {
            int i_param = j + 4 * i_finger;
            (*positions_d)[i_param] +=
                ((transforms_d[i][j] * model.base_positions.colwise().homogeneous()).array()
                    .rowwise() * model.weights.row(i)).matrix()
                .topRows(3);
        }
    }

    if (model.is_mirrored)
        positions->row(0) = -positions->row(0);
}

void get_skinned_vertex_positions_d(
    const HandModelEigen& model,
    const Matrix3Xd& pose_params,
    const vector<int>& corresp,
    Matrix3Xd* positions,
    double* pJ,
    bool apply_global)
{
    vector<Matrix3Xd> positions_d;
    get_skinned_vertex_positions_d_common(model, pose_params, corresp, positions,
        &positions_d, pJ, apply_global);

    Matrix3d Rglob = Matrix3d::Identity();
    if (apply_global)
        apply_global_transform_d(corresp, pose_params, positions, pJ, &Rglob);

    // finger parameters
    size_t ncorresp = corresp.size();
    for (int i = 0; i < 4 * 5; i++)
    {
        Map<Matrix3Xd> curr_J(&pJ[(6 + i) * 3 * ncorresp], 3, ncorresp);// 6 is offset (global params)
        for (int j = 0; j < curr_J.cols(); j++)
        {
            curr_J.col(j).noalias() = -Rglob * positions_d[i].col(corresp[j]);
        }
    }
}

void get_skinned_vertex_positions_d(
    const double* const us,
    const HandModelEigen& model,
    const Matrix3Xd& pose_params,
    const vector<int>& corresp,
    Matrix3Xd* positions,
    double* pJ,
    bool apply_global)
{
    vector<Matrix3Xd> positions_d;

    get_skinned_vertex_positions_d_common(model, pose_params, corresp, positions,
        &positions_d, pJ, apply_global);

    Matrix3d Rglob = Matrix3d::Identity();
    if (apply_global)
        apply_global_transform_d(us, model.triangles, corresp, pose_params, positions, pJ, &Rglob);

    // finger parameters
    size_t ncorresp = corresp.size();
    Vector3d tmp;
    for (int i = 0; i < 4 * 5; i++)
    {
        Map<Matrix3Xd> curr_J(&pJ[(6 + i) * 3 * ncorresp], 3, ncorresp); // 6 is offset (global params)
        for (int j = 0; j < curr_J.cols(); j++)
        {
            const auto& verts = model.triangles[corresp[j]].verts;
            const double* const u = &us[2 * j];

            tmp = u[0] * positions_d[i].col(verts[0]) + u[1] * positions_d[i].col(verts[1])
                + (1. - u[0] - u[1]) * positions_d[i].col(verts[2]);

            curr_J.col(j).noalias() = -Rglob * tmp;
        }
    }
}

void to_pose_params_d(const double* const theta,
    const vector<string>& bone_names,
    Matrix3Xd* ppose_params)
{
    auto& pose_params = *ppose_params;
    pose_params.resize(3, bone_names.size() + 3);
    pose_params.setZero();

    pose_params.col(0) = Map<const Vector3d>(&theta[0]);
    pose_params.col(1).setOnes();
    pose_params.col(2) = Map<const Vector3d>(&theta[3]);

    int i_theta = 6;
    int i_pose_params = 5;
    int n_fingers = 5;
    for (int i_finger = 0; i_finger < n_fingers; i_finger++)
    {
        for (int i = 2; i <= 4; i++)
        {
            pose_params(0, i_pose_params) = theta[i_theta++];
            if (i == 2)
            {
                pose_params(1, i_pose_params) = theta[i_theta++];
            }
            i_pose_params++;
        }
        i_pose_params++;
    }
}

void hand_objective_d(
    const double* const theta,
    const HandDataEigen& data,
    double* perr,
    double* pJ)
{
    Matrix3Xd pose_params;
    to_pose_params_d(theta, data.model.bone_names, &pose_params);

    Matrix3Xd vertex_positions;
    get_skinned_vertex_positions_d(data.model, pose_params, data.correspondences, &vertex_positions, pJ);

    size_t npts = data.correspondences.size();
    Map<Matrix3Xd> err(perr, 3, npts);
    for (size_t i = 0; i < data.correspondences.size(); i++)
    {
        err.col(i) = data.points.col(i) - vertex_positions.col(data.correspondences[i]);
    }
}

void hand_objective_d(
    const double* const theta,
    const double* const us,
    const HandDataEigen& data,
    double* perr,
    double* pJ)
{
    Matrix3Xd pose_params;
    to_pose_params_d(theta, data.model.bone_names, &pose_params);

    size_t npts = data.correspondences.size();
    Matrix3Xd vertex_positions;
    get_skinned_vertex_positions_d(us, data.model, pose_params, data.correspondences, &vertex_positions, &pJ[2 * 3 * npts]);

    Map<Matrix3Xd> err(perr, 3, npts);
    Map<Matrix3Xd> du0(&pJ[0], 3, npts), du1(&pJ[3 * npts], 3, npts);
    Vector3d hand_point;
    for (size_t i = 0; i < data.correspondences.size(); i++)
    {
        const auto& verts = data.model.triangles[data.correspondences[i]].verts;
        const double* const u = &us[2 * i];

        du0.col(i) = -(vertex_positions.col(verts[0]) - vertex_positions.col(verts[2]));
        du1.col(i) = -(vertex_positions.col(verts[1]) - vertex_positions.col(verts[2]));

        hand_point = u[0] * vertex_positions.col(verts[0]) + u[1] * vertex_positions.col(verts[1])
            + (1. - u[0] - u[1]) * vertex_positions.col(verts[2]);
        err.col(i) = data.points.col(i) - hand_point;
    }
}


// Copyright (C) 2017  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/global_optimization.h>
#include <dlib/statistics.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>
#include <dlib/rand.h>

#include "tester.h"

namespace
{

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

    logger dlog("test.global_optimization");

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

    void test_upper_bound_function(double relative_noise_magnitude, double solver_eps)
    {
        print_spinner();

        dlog << LINFO << "test_upper_bound_function,  relative_noise_magnitude="<< relative_noise_magnitude  << ", solver_eps=" << solver_eps;

        auto rosen = [](const matrix<double,0,1>& x) { return -1*( 100*std::pow(x(1) - x(0)*x(0),2.0) + std::pow(1 - x(0),2)); };

        dlib::rand rnd;
        auto make_rnd = [&rnd]() { matrix<double,0,1> x(2); x = 2*rnd.get_random_double(), 2*rnd.get_random_double(); return x; };

        std::vector<function_evaluation> evals;
        for (int i = 0; i < 100; ++i)
        {
            auto x = make_rnd();
            evals.emplace_back(x,rosen(x));
        }

        upper_bound_function ub(evals, relative_noise_magnitude, solver_eps);
        DLIB_TEST(ub.num_points() == (long)evals.size());
        DLIB_TEST(ub.dimensionality() == 2);
        for (auto& ev : evals)
        {
            dlog << LINFO << ub(ev.x) - ev.y;
            DLIB_TEST_MSG(ub(ev.x) - ev.y > -1e10, ub(ev.x) - ev.y);
        }

        for (int i = 0; i < 100; ++i)
        {
            auto x = make_rnd();
            evals.emplace_back(x,rosen(x));
            ub.add(evals.back());
        }

        DLIB_TEST(ub.num_points() == (long)evals.size());
        DLIB_TEST(ub.dimensionality() == 2);

        for (auto& ev : evals)
        {
            dlog << LINFO << ub(ev.x) - ev.y;
            DLIB_TEST_MSG(ub(ev.x) - ev.y > -1e10, ub(ev.x) - ev.y);
        }

        if (solver_eps < 0.001)
        {
            dlog << LINFO << "out of sample points: ";
            for (int i = 0; i < 10; ++i)
            {
                auto x = make_rnd();
                dlog << LINFO << ub(x) - rosen(x);
                DLIB_TEST_MSG(ub(x) - rosen(x) > 1e-10, ub(x) - rosen(x));
            }
        }
    }

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

    double complex_holder_table ( double x0, double x1)
    {
        // The regular HolderTable function
        //return -std::abs(sin(x0)*cos(x1)*exp(std::abs(1-std::sqrt(x0*x0+x1*x1)/pi)));

        // My more complex version of it with discontinuities and more local minima.
        double sign = 1;
        for (double j = -4; j < 9; j += 0.5)
        {
            if (j < x0 && x0 < j+0.5)
                x0 += sign*0.25;
            sign *= -1;
        }
        // HolderTable function tilted towards 10,10
        return -std::abs(sin(x0)*cos(x1)*exp(std::abs(1-std::sqrt(x0*x0+x1*x1)/pi))) +(x0+x1)/10 + sin(x0*10)*cos(x1*10);
    }

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

    void test_global_function_search()
    {

        function_spec spec{{-10,-10}, {10,10}};
        function_spec spec2{{-10,-10, -50}, {10,10, 50}};
        global_function_search opt({spec, spec, spec2});

        dlib::rand rnd;
        bool found_optimal_point = false;
        for (int i = 0; i < 400 && !found_optimal_point; ++i)
        {
            print_spinner();
            std::vector<function_evaluation_request> nexts;
            for (int k = 0; k < rnd.get_integer_in_range(1,4); ++k)
                nexts.emplace_back(opt.get_next_x());

            for (auto& next : nexts)
            {
                switch (next.function_idx())
                {
                    case 0: next.set( -complex_holder_table(next.x()(0), next.x()(1))); break;
                    case 1: next.set( -10*complex_holder_table(next.x()(0), next.x()(1))); break;
                    case 2: next.set( -2*complex_holder_table(next.x()(0), next.x()(1))); break;
                    default: DLIB_TEST(false); break;
                }

                matrix<double,0,1> x;
                double y;
                size_t function_idx;
                opt.get_best_function_eval(x,y,function_idx);
                /*
                cout << "\ni: "<< i << endl;
                cout << "best eval x: "<< trans(x);
                cout << "best eval y: "<< y << endl;
                cout << "best eval function index: "<< function_idx << endl;
                */

                if (std::abs(y  - 10*21.9210397) < 0.0001)
                {
                    found_optimal_point = true;
                    break;
                }
            }
        }

        DLIB_TEST(found_optimal_point);
    }

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

    void test_find_max_global(
    )
    {
        print_spinner();
        auto rosen = [](const matrix<double,0,1>& x) { return -1*( 100*std::pow(x(1) - x(0)*x(0),2.0) + std::pow(1 - x(0),2)); };

        auto result = find_max_global(rosen, {0.1, 0.1}, {2, 2}, max_function_calls(100), 0);
        matrix<double,0,1> true_x = {1,1};

        dlog << LINFO << "rosen: " <<  trans(result.x);
        DLIB_TEST_MSG(max(abs(true_x-result.x)) < 1e-5, max(abs(true_x-result.x)));
        print_spinner();

        result = find_max_global(rosen, {0.1, 0.1}, {2, 2}, max_function_calls(100));
        dlog << LINFO << "rosen: " <<  trans(result.x);
        DLIB_TEST_MSG(max(abs(true_x-result.x)) < 1e-5, max(abs(true_x-result.x)));
        print_spinner();

        result = find_max_global(rosen, {0.1, 0.1}, {2, 2}, max_function_calls(100), 0, {function_evaluation({1.1, 0.9}, rosen({1.1, 0.9}))});
        dlog << LINFO << "rosen: " <<  trans(result.x);
        DLIB_TEST_MSG(max(abs(true_x-result.x)) < 1e-5, max(abs(true_x-result.x)));
        print_spinner();

        result = find_max_global(rosen, {0.1, 0.1}, {2, 2}, std::chrono::seconds(5));
        dlog << LINFO << "rosen: " <<  trans(result.x);
        DLIB_TEST_MSG(max(abs(true_x-result.x)) < 1e-5, max(abs(true_x-result.x)));
        print_spinner();

        result = find_max_global(rosen, {0.1, 0.1}, {2, 2}, {false,false}, max_function_calls(100));
        dlog << LINFO << "rosen: " <<  trans(result.x);
        DLIB_TEST_MSG(max(abs(true_x-result.x)) < 1e-5, max(abs(true_x-result.x)));
        print_spinner();

        result = find_max_global(rosen, {0.1, 0.1}, {0.9, 0.9}, {false,false}, max_function_calls(140));
        true_x = {0.9, 0.81};
        dlog << LINFO << "rosen, bounded at 0.9: " <<  trans(result.x);
        DLIB_TEST_MSG(max(abs(true_x-result.x)) < 1e-5, max(abs(true_x-result.x)));
        print_spinner();

        result = find_max_global([](double x){ return -std::pow(x-2,2.0); }, -10, 10, max_function_calls(10), 0);
        dlog << LINFO << "(x-2)^2: " <<  trans(result.x);
        DLIB_TEST(result.x.size()==1);
        DLIB_TEST(std::abs(result.x - 2) < 1e-9);
        print_spinner();

        result = find_max_global([](double x){ return -std::pow(x-2,2.0); }, -10, 1, max_function_calls(10));
        dlog << LINFO << "(x-2)^2, bound at 1: " <<  trans(result.x);
        DLIB_TEST(result.x.size()==1);
        DLIB_TEST(std::abs(result.x - 1) < 1e-9);
        print_spinner();

        result = find_max_global([](double x){ return -std::pow(x-2,2.0); }, -10, 1, std::chrono::seconds(2));
        dlog << LINFO << "(x-2)^2, bound at 1: " <<  trans(result.x);
        DLIB_TEST(result.x.size()==1);
        DLIB_TEST(std::abs(result.x - 1) < 1e-9);
        print_spinner();

        result = find_max_global([](double a, double b){ return -complex_holder_table(a,b);},
            {-10, -10}, {10, 10}, max_function_calls(400), 0);
        dlog << LINFO << "complex_holder_table y: "<< result.y;
        DLIB_TEST_MSG(std::abs(result.y  - 21.9210397) < 0.0001, std::abs(result.y  - 21.9210397));
    }

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

    void test_find_min_global(
    )
    {
        print_spinner();
        auto rosen = [](const matrix<double,0,1>& x) { return +1*( 100*std::pow(x(1) - x(0)*x(0),2.0) + std::pow(1 - x(0),2)); };

        auto result = find_min_global(rosen, {0.1, 0.1}, {2, 2}, max_function_calls(100), 0);
        matrix<double,0,1> true_x = {1,1};

        dlog << LINFO << "rosen: " <<  trans(result.x);
        DLIB_TEST_MSG(min(abs(true_x-result.x)) < 1e-5, min(abs(true_x-result.x)));
        print_spinner();

        result = find_min_global(rosen, {0.1, 0.1}, {2, 2}, max_function_calls(100));
        dlog << LINFO << "rosen: " <<  trans(result.x);
        DLIB_TEST_MSG(min(abs(true_x-result.x)) < 1e-5, min(abs(true_x-result.x)));
        print_spinner();

        result = find_min_global(rosen, {0.1, 0.1}, {2, 2}, std::chrono::seconds(5));
        dlog << LINFO << "rosen: " <<  trans(result.x);
        DLIB_TEST_MSG(min(abs(true_x-result.x)) < 1e-5, min(abs(true_x-result.x)));
        print_spinner();

        result = find_min_global(rosen, {0.1, 0.1}, {2, 2}, {false,false}, max_function_calls(100));
        dlog << LINFO << "rosen: " <<  trans(result.x);
        DLIB_TEST_MSG(min(abs(true_x-result.x)) < 1e-5, min(abs(true_x-result.x)));
        print_spinner();

        result = find_min_global(rosen, {0, 0}, {0.9, 0.9}, {false,false}, max_function_calls(100));
        true_x = {0.9, 0.81};
        dlog << LINFO << "rosen, bounded at 0.9: " <<  trans(result.x);
        DLIB_TEST_MSG(min(abs(true_x-result.x)) < 1e-5, min(abs(true_x-result.x)));
        print_spinner();

        result = find_min_global([](double x){ return std::pow(x-2,2.0); }, -10, 10, max_function_calls(10), 0);
        dlog << LINFO << "(x-2)^2: " <<  trans(result.x);
        DLIB_TEST(result.x.size()==1);
        DLIB_TEST(std::abs(result.x - 2) < 1e-9);
        print_spinner();

        result = find_min_global([](double x){ return std::pow(x-2,2.0); }, -10, 1, max_function_calls(10));
        dlog << LINFO << "(x-2)^2, bound at 1: " <<  trans(result.x);
        DLIB_TEST(result.x.size()==1);
        DLIB_TEST(std::abs(result.x - 1) < 1e-9);
        print_spinner();

        result = find_min_global([](double x){ return std::pow(x-2,2.0); }, -10, 1, std::chrono::seconds(2));
        dlog << LINFO << "(x-2)^2, bound at 1: " <<  trans(result.x);
        DLIB_TEST(result.x.size()==1);
        DLIB_TEST(std::abs(result.x - 1) < 1e-9);
        print_spinner();

        result = find_min_global([](double a, double b){ return complex_holder_table(a,b);},
            {-10, -10}, {10, 10}, max_function_calls(400), 0);
        dlog << LINFO << "complex_holder_table y: "<< result.y;
        DLIB_TEST_MSG(std::abs(result.y  + 21.9210397) < 0.0001, std::abs(result.y  + 21.9210397));
    }

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

    class global_optimization_tester : public tester
    {
    public:
        global_optimization_tester (
        ) :
            tester ("test_global_optimization",
                    "Runs tests on the global optimization components.")
        {}

        void perform_test (
        )
        {
            test_upper_bound_function(0.01, 1e-6);
            test_upper_bound_function(0.0, 1e-6);
            test_upper_bound_function(0.0, 1e-1);
            test_global_function_search();
            test_find_max_global();
            test_find_min_global();
        }
    } a;

}


#pragma once

#include <cstdint>
#include <Eigen/Dense>

/** @class RobotEstimator
 *  This class implements a Kalman Filter in order to estimate a robot's current state.
 *  The estimated state is then used in motion control to assign wheel velocities accordingly
 *
 *  Explanations:
 *  - https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
 *  - https://en.wikipedia.org/wiki/Kalman_filter
 *  - https://www.mathworks.com/videos/series/understanding-kalman-filters.html
 */
class RobotEstimator {
private:
    /**
     * Number of tracked states (number of states in state vector, x_hat)
     *
     * 3 Total:
     * - X and Y: linear velocities of robot body (m/s)
     * - W: angular velocity of robot body around z axis (rad/s)
     */
    static constexpr int numStates = 3;

    /**
     * Number of applied inputs (number of control inputs in control vector, u)
     *
     * 4 total:
     * - Motor duty cycle for each motor (% max), controls average applied voltage
     */
    static constexpr int numInputs = 4;

    /**
     * Number of tracked measurable outputs (number of outputs in output vector, y)
     *
     * 5 total:
     * - 4 wheel motor encoders (rad/s)
     * - Gyro angular velocity (rad/s)
     */
    static constexpr int numOutputs = 5;

public:
    /**
     * @param dt_us Expected period of the controller in us
     */
    RobotEstimator(uint32_t dt_us);

    /**
     * Using the previous state and the next input
     * We can guess where we are this time step
     *
     * @param u Last motor command
     */
    void predict(Eigen::Matrix<float, numInputs, 1> u);

    /**
     * Using the next measurements, we can move our prediction
     * closer to the true target
     *
     * @param z Encoders 1-4 then gyro
     */
    void update(Eigen::Matrix<float, numOutputs, 1> z);

    /**
     * @param state Matrix that the current guess will be saved into
     */
    void getState(Eigen::Matrix<float, numStates, 1>& state);

private:

    static constexpr float processNoise = 0.05;  /**< State noise from unmodeled influences (model errors/omissions) */
    static constexpr float encoderNoise = 0.04;  /**< Measurement noise in the encoder */
    static constexpr float gyroNoise = 0.005;    /**< Measurement noise in the gyro */
    static constexpr float initCovariance = 0.1; /**< The initial covariance value in each entry of `P` */

    /**
     * State Transition Matrix/Prediction Matrix
     *
     * Predict our next state based on our current state using dynamics
     */
    Eigen::Matrix<float, numStates,  numStates>  F;

    /**
     * Control Input Matrix
     *
     * Corrects our next state prediction by mapping control input (known influences, `u`) to our next state
     */
    Eigen::Matrix<float, numStates,  numInputs>  B;

    /**
     * Observation Matrix
     *
     * Predicts the measurements from our sensors (`z`) from our current state (`x_hat`).
     */
    Eigen::Matrix<float, numOutputs, numStates>  H;

    /**
     * Covariance Matrix for Process Noise
     *
     * A matrix whose entries are the estimated covariances between any two state variables.
     * Entries represent uncertainty in our state vector (`x_hat`) due to unmodeled influences.
     */
    Eigen::Matrix<float, numStates,  numStates>  Q;

    /**
     * Covariance Matrix for Observation Noise
     *
     * A matrix whose entries are the estimated covariances between any two state variables.
     * Entries represent uncertainty in our measurements (`z`) due to random noise
     */
    Eigen::Matrix<float, numOutputs, numOutputs> R;

    /**
     * Covariance Estimate Matrix
     *
     * A matrix whose entries are the estimated covariances between any two state variables.
     * Entries represent our overall uncertainty in our state vector (`x_hat`).
     */
    Eigen::Matrix<float, numStates,  numStates>  P;

    /**
     * Identity Matrix
     *
     * A square matrix with only 1s on the diagonals
     */
    Eigen::Matrix<float, numStates, numStates> I;

    /**
     * Current State Vector Estimate
     *
     * What the robot estimator believes our current state to be
     */
    Eigen::Matrix<float, numStates, 1> x_hat;
};


/**
 *          Copyright Matthias Walter 2010.
 * 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 "../config.h"
#include <map>
#include <vector>

#include <boost/numeric/ublas/lu.hpp>
#include <boost/dynamic_bitset.hpp>

#include "total_unimodularity.hpp"
#include "combinations.hpp"

namespace unimod
{

  /**
   * Calculates a subdeterminant of the given matrix.
   *
   * @param matrix A given integer matrix
   * @param submatrix Matrix-indices describing a submatrix
   * @return The submatrix' determinant
   */

  int submatrix_determinant(const integer_matrix& matrix, const submatrix_indices& submatrix)
  {
    typedef boost::numeric::ublas::matrix_indirect <const integer_matrix, submatrix_indices::indirect_array_type> indirect_matrix_t;

    assert (submatrix.rows.size() == submatrix.columns.size());

    const indirect_matrix_t indirect_matrix(matrix, submatrix.rows, submatrix.columns);

    boost::numeric::ublas::matrix <float> float_matrix(indirect_matrix);
    boost::numeric::ublas::permutation_matrix <size_t> permutation_matrix(float_matrix.size1());

    /// LU-factorize
    int result = boost::numeric::ublas::lu_factorize(float_matrix, permutation_matrix);
    if (result != 0)
      return 0;

    /// Calculate the determinant as a product of the diagonal entries, taking the permutation matrix into account.
    float det = 1.0f;
    for (size_t i = 0; i < float_matrix.size1(); ++i)
    {
      det *= float_matrix(i, i);
      if (i != permutation_matrix(i))
        det = -det;
    }

    return (int) det;
  }

  /**
   * Checks Camion's criterion for total unimodularity.
   *
   * @param matrix A given integer matrix
   * @param submatrix Matrix-indices describing a submatrix B of A
   * @return true iff B is not Eulerian or 1^T * B * 1 = 0 (mod 4)
   */

  bool submatrix_camion(const integer_matrix& matrix, const submatrix_indices& submatrix)
  {
    typedef boost::numeric::ublas::matrix_indirect <const integer_matrix, submatrix_indices::indirect_array_type> indirect_matrix_t;

    assert (submatrix.rows.size() == submatrix.columns.size());

    const indirect_matrix_t indirect_matrix(matrix, submatrix.rows, submatrix.columns);

    // Test whether submatrix is eulerian.
    for (size_t row = 0; row < indirect_matrix.size1(); ++row)
    {
      size_t count = 0;
      for (size_t column = 0; column < indirect_matrix.size2(); ++column)
        count += indirect_matrix(row, column);
      if (count % 2 == 1)
        return true;
    }
    for (size_t column = 0; column < indirect_matrix.size2(); ++column)
    {
      size_t count = 0;
      for (size_t row = 0; row < indirect_matrix.size1(); ++row)
        count += indirect_matrix(row, column);
      if (count % 2 == 1)
        return true;
    }

    // Matrix is eulerian.
    size_t count = 0;
    for (size_t row = 0; row < indirect_matrix.size1(); ++row)
    {
      for (size_t column = 0; column < indirect_matrix.size2(); ++column)
        count += indirect_matrix(row, column);
    }
    return (count % 4 == 0);
  }

  /**
   * Checks all subdeterminants to test a given matrix for total unimodularity.
   *
   * @param matrix The given matrix
   * @return true if and only if this matrix is totally unimdular
   */

  bool determinant_is_totally_unimodular(const integer_matrix& matrix)
  {
    submatrix_indices indices;

    return determinant_is_totally_unimodular(matrix, indices);
  }

  /**
   * Checks all subdeterminants to test a given matrix for total unimodularity.
   * If this is not the case, violator describes a violating submatrix.
   *
   * @param matrix The given matrix
   * @param violator The violating submatrix, if the result is false
   * @return true if and only if the this matrix is totally unimodular
   */

  bool determinant_is_totally_unimodular(const integer_matrix& matrix, submatrix_indices& violator)
  {
    for (size_t size = 1; size <= matrix.size1(); ++size)
    {
      combination row_combination(matrix.size1(), size);
      while (true)
      {
        combination column_combination(matrix.size2(), size);
        while (true)
        {
          submatrix_indices sub;
          submatrix_indices::vector_type indirect_array(size);
          for (size_t i = 0; i < size; ++i)
            indirect_array[i] = row_combination[i];
          sub.rows = submatrix_indices::indirect_array_type(size, indirect_array);
          for (size_t i = 0; i < size; ++i)
          {
            indirect_array[i] = column_combination[i];
          }
          sub.columns = submatrix_indices::indirect_array_type(size, indirect_array);

          /// Examine the determinant
          bool camion = submatrix_camion(matrix, sub);

#ifndef NDEBUG
          int det = submatrix_determinant(matrix, sub);
          bool is_violator = det < -1 || det > +1;
          assert((is_violator && !camion) || (!is_violator && camion));
#pragma message("\n\n\nWARNING: Submatrix Test uses subdeterminants for verification and is much slower!\n\n ")
#endif
          if (!camion)
          {
            violator = sub;
            return false;
          }

          if (column_combination.is_last())
            break;
          column_combination.next();
        }
        if (row_combination.is_last())
          break;
        row_combination.next();
      }
    }
    return true;
  }

}


/*=============================================================================

  SKSURGERYOPENCVCPP: Image-guided surgery functions, in C++, using OpenCV.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "sksTriangulate.h"
#include "sksExceptionMacro.h"

#include <opencv2/calib3d.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <iostream>

namespace sks
{

//-----------------------------------------------------------------------------
double Norm(const cv::Point3d& p1)
{
  return std::sqrt(p1.x * p1.x + p1.y * p1.y + p1.z * p1.z);
}

//-----------------------------------------------------------------------------
cv::Point3d CrossProduct(const cv::Point3d& p1, const cv::Point3d& p2)
{
  cv::Point3d cp;
  cp.x = p1.y * p2.z - p1.z * p2.y;
  cp.y = p1.z * p2.x - p1.x * p2.z;
  cp.z = p1.x * p2.y - p1.y * p2.x;
  return cp;
}

//-----------------------------------------------------------------------------
double DotProduct(const cv::Point3d& p1, const cv::Point3d& p2)
{
  return p1.x * p2.x + p1.y * p2.y + p1.z * p2.z;
}

//-----------------------------------------------------------------------------
double DistanceToLine(const std::pair<cv::Point3d, cv::Point3d>& line, const cv::Point3d& x0 )
{
  // Courtesy of Wolfram Mathworld
  cv::Point3d x1;
  cv::Point3d x2;

  x1 = line.first;
  x2 = line.second;

  cv::Point3d d1 = x1-x0;
  cv::Point3d d2 = x2-x1;

  return sks::Norm(sks::CrossProduct(d2, d1)) / (sks::Norm(d2));
}

//-----------------------------------------------------------------------------
double DistanceBetweenLines(const cv::Point3d& P0, const cv::Point3d& u,
                            const cv::Point3d& Q0, const cv::Point3d& v,
                            cv::Point3d& midpoint)
{
  // Method 1. Solve for shortest line joining two rays, then get midpoint.
  // Taken from: http://geomalgorithms.com/a07-_distance.html
  double sc, tc, a, b, c, d, e;
  double distance;

  cv::Point3d Psc;
  cv::Point3d Qtc;
  cv::Point3d W0;

  // Difference of two origins
  W0.x = P0.x - Q0.x;
  W0.y = P0.y - Q0.y;
  W0.z = P0.z - Q0.z;

  a = u.x*u.x + u.y*u.y + u.z*u.z;
  b = u.x*v.x + u.y*v.y + u.z*v.z;
  c = v.x*v.x + v.y*v.y + v.z*v.z;
  d = u.x*W0.x + u.y*W0.y + u.z*W0.z;
  e = v.x*W0.x + v.y*W0.y + v.z*W0.z;
  sc = (b*e - c*d) / (a*c - b*b);
  tc = (a*e - b*d) / (a*c - b*b);

  if ( boost::math::isnan(sc) || boost::math::isnan(tc) || boost::math::isinf(sc) || boost::math::isinf(tc) )
  {
    // Lines are parallel
    distance = sks::DistanceToLine ( std::pair<cv::Point3d, cv::Point3d> ( P0, P0 + u ), Q0 );
    midpoint.x = std::numeric_limits<double>::quiet_NaN();
    midpoint.y = std::numeric_limits<double>::quiet_NaN();
    midpoint.z = std::numeric_limits<double>::quiet_NaN();
    return distance;
  }
  Psc.x = P0.x + sc*u.x;
  Psc.y = P0.y + sc*u.y;
  Psc.z = P0.z + sc*u.z;
  Qtc.x = Q0.x + tc*v.x;
  Qtc.y = Q0.y + tc*v.y;
  Qtc.z = Q0.z + tc*v.z;

  distance = std::sqrt((Psc.x - Qtc.x)*(Psc.x - Qtc.x)
                        +(Psc.y - Qtc.y)*(Psc.y - Qtc.y)
                        +(Psc.z - Qtc.z)*(Psc.z - Qtc.z));

  midpoint.x = (Psc.x + Qtc.x)/2.0;
  midpoint.y = (Psc.y + Qtc.y)/2.0;
  midpoint.z = (Psc.z + Qtc.z)/2.0;

  return distance;
}

//-----------------------------------------------------------------------------
double ComputeRMSBetweenCorrespondingPoints(const cv::Mat& a, const cv::Mat& b)
{
  double rms = 0;
  double diff = 0;
  double squaredDiff = 0;

  if (a.rows != b.rows)
  {
    sksExceptionThrow() << "a has " << a.rows << " rows, but b has " << b.rows;
  }

  if (a.cols != 3)
  {
    sksExceptionThrow() << "a does not have 3 columns.";
  }

  if (b.cols != 3)
  {
    sksExceptionThrow() << "b does not have 3 columns.";
  }

  for (unsigned int r = 0; r < a.rows; r++)
  {
    for (unsigned int c = 0; c < a.cols; c++)
    {
      diff = (b.at<double>(r, c) - a.at<double>(r, c));
      squaredDiff = diff * diff;
      rms += squaredDiff;
    }
  }
  rms /= static_cast<double>(a.rows);
  rms = std::sqrt(rms);
  return rms;
}

} // end namespace


/*
Copyright 2020 Standard Cyborg

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
#include "standard_cyborg/algorithms/PointToPointAlignment.hpp"

#include "standard_cyborg/util/IncludeEigen.hpp"

#include "standard_cyborg/math/Vec3.hpp"
#include "standard_cyborg/math/Mat3x4.hpp"

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdocumentation"
#include <Eigen/Jacobi>
#pragma clang diagnostic pop

namespace standard_cyborg {

namespace algorithms {

standard_cyborg::math::Mat3x4 PointToPointAlignment(const std::vector<standard_cyborg::math::Vec3>& sourcePositions, const std::vector<standard_cyborg::math::Vec3>& targetPositions)
{
    int N = (int)sourcePositions.size();

    Eigen::Matrix3Xf p = Eigen::Matrix3Xf(3, N);
    Eigen::Matrix3Xf q = Eigen::Matrix3Xf(3, N);
    Eigen::MatrixXf avgP, avgQ, pNorm, qNorm, C, R, t;

    for (int iv = 0; iv < N; ++iv) {
        p.col(iv) = Eigen::Vector3f(sourcePositions[iv].x, sourcePositions[iv].y, sourcePositions[iv].z);
        q.col(iv) = Eigen::Vector3f(targetPositions[iv].x, targetPositions[iv].y, targetPositions[iv].z);
    }

    avgP = p.rowwise().mean();
    avgQ = q.rowwise().mean();

    pNorm = p - avgP.replicate(1, p.cols());
    qNorm = q - avgQ.replicate(1, q.cols());

    C = pNorm * qNorm.transpose();
    Eigen::JacobiSVD<Eigen::MatrixXf> svd(C, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // rotation
    R = svd.matrixV() * svd.matrixU().transpose();

    // translation
    t = avgQ - R * avgP;

    standard_cyborg::math::Mat3x4 r = {
        R(0, 0), R(0, 1), R(0, 2), t(0),
        R(1, 0), R(1, 1), R(1, 2), t(1),
        R(2, 0), R(2, 1), R(2, 2), t(2),
    };

    return r;
}

}

}


//////////////////////////////////////////////////////////////////////////////
// monomials_horner::monomials_properties.hpp                               //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  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_MONOMIALS_HORNER_MONOMIALS_PROPERTIES_HPP_ER_2009
#define BOOST_MONOMIALS_HORNER_MONOMIALS_PROPERTIES_HPP_ER_2009
#include <boost/math/special_functions/binomial.hpp>

namespace boost{
namespace monomials_horner{

    template<typename T = double>
    struct monomials_properties{
        static unsigned long number_degree_less_than(
            unsigned the_degree,unsigned dim){
                return static_cast<unsigned long>(math::binomial_coefficient<T>(
                    the_degree+dim,dim));
        }
    };

}// monomials_horner
}// boost

#endif


/**
 * \file Chebyshev1Filter.cpp
 */

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

#include "Chebyshev1Filter.h"
#include "helpers.h"
#include "IIRFilter.h"

namespace
{
  template<typename DataType>
  void create_chebyshev1_analog_coefficients(int order, DataType ripple, std::vector<std::complex<DataType> >& z, std::vector<std::complex<DataType> >& p, DataType& k)
  {
    z.clear(); // no zeros for this filter type
    p.clear();
    if(ripple == 0)
    {
      return;
    }
    if(order == 0)
    {
      k = static_cast<DataType>(std::pow(10, (-ripple / 20)));
      return;
    }

    DataType eps = static_cast<DataType>(std::sqrt(std::pow(10, (0.1 * ripple)) - 1.0));
    DataType mu = static_cast<DataType>(1.0 / order * boost::math::asinh(1 / eps));

    for(int i = -order+1; i < order; i += 2)
    {
      DataType theta = boost::math::constants::pi<DataType>() * i / (2*order);
      p.push_back(-std::sinh(std::complex<DataType>(mu, theta)));
    }

    std::complex<DataType> f = 1;

    for(int i = 0; i < p.size(); ++i)
    {
      f *= -p[i];
    }
    k = f.real();
    if(order % 2 == 0)
    {
      k = k / std::sqrt((1 + eps * eps));
    }
  }

  template<typename DataType>
  void create_default_chebyshev1_coeffs(size_t order, DataType ripple, DataType Wn, std::vector<DataType>& coefficients_in, std::vector<DataType>& coefficients_out)
  {
    std::vector<std::complex<DataType> > z;
    std::vector<std::complex<DataType> > p;
    DataType k;

    int fs = 2;
    create_chebyshev1_analog_coefficients(static_cast<int>(order), ripple, z, p, k);
    DataType warped = 2 * fs * std::tan(boost::math::constants::pi<DataType>() *  Wn / fs);
    zpk_lp2lp(warped, z, p, k);
    zpk_bilinear(fs, z, p, k);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    zpk2ba(fs, z, p, k, b, a);

    for(size_t i = 0; i < std::min(order + 1, b.size()); ++i)
    {
      coefficients_in[i] = b[i];
    }
    for(size_t i = 0; i < std::min(order, a.size()-1); ++i)
    {
      coefficients_out[i] = -a[i];
    }
  }

  template<typename DataType>
  void create_bp_chebyshev1_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, std::vector<DataType>& coefficients_in, std::vector<DataType>& coefficients_out)
  {
    std::vector<std::complex<DataType> > z;
    std::vector<std::complex<DataType> > p;
    DataType k;

    int fs = 2;
    create_chebyshev1_analog_coefficients(static_cast<int>(order/2), ripple, z, p, k);
    wc1 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc1 / fs);
    wc2 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc2 / fs);

    zpk_lp2bp(std::sqrt(wc1 * wc2), wc2 - wc1, z, p, k);
    zpk_bilinear(fs, z, p, k);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    zpk2ba(fs, z, p, k, b, a);

    for(size_t i = 0; i < std::min(order + 1, b.size()); ++i)
    {
      coefficients_in[i] = b[i];
    }
    for(size_t i = 0; i < std::min(order, a.size()-1); ++i)
    {
      coefficients_out[i] = -a[i];
    }
  }

  template<typename DataType>
  void create_bs_chebyshev1_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, std::vector<DataType>& coefficients_in, std::vector<DataType>& coefficients_out)
  {
    std::vector<std::complex<DataType> > z;
    std::vector<std::complex<DataType> > p;
    DataType k;

    int fs = 2;
    create_chebyshev1_analog_coefficients(static_cast<int>(order/2), ripple, z, p, k);
    wc1 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc1 / fs);
    wc2 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc2 / fs);

    zpk_lp2bs(std::sqrt(wc1 * wc2), wc2 - wc1, z, p, k);
    zpk_bilinear(fs, z, p, k);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    zpk2ba(fs, z, p, k, b, a);

    for(size_t i = 0; i < std::min(order + 1, b.size()); ++i)
    {
      coefficients_in[i] = b[i];
    }
    for(size_t i = 0; i < std::min(order, a.size()-1); ++i)
    {
      coefficients_out[i] = -a[i];
    }
  }
}

namespace ATK
{
  template <typename DataType>
  Chebyshev1LowPassCoefficients<DataType>::Chebyshev1LowPassCoefficients(int nb_channels)
  :Parent(nb_channels, nb_channels), cut_frequency(0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev1LowPassCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev1LowPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType_>
  void Chebyshev1LowPassCoefficients<DataType_>::set_cut_frequency(DataType_ cut_frequency)
  {
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev1LowPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType>
  void Chebyshev1LowPassCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  void Chebyshev1LowPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_default_chebyshev1_coeffs(in_order, ripple, 2 * cut_frequency / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev1HighPassCoefficients<DataType>::Chebyshev1HighPassCoefficients(int nb_channels)
  :Parent(nb_channels, nb_channels), cut_frequency(0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev1HighPassCoefficients<DataType_>::set_cut_frequency(DataType_ cut_frequency)
  {
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev1HighPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType_>
  void Chebyshev1HighPassCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev1HighPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev1HighPassCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  void Chebyshev1HighPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_default_chebyshev1_coeffs(in_order, ripple, (input_sampling_rate - 2 * cut_frequency) / input_sampling_rate, coefficients_in, coefficients_out);
    for(int i = in_order - 1; i >= 0; i -= 2)
    {
      coefficients_in[i] = - coefficients_in[i];
      coefficients_out[i] = - coefficients_out[i];
    }
  }

  template <typename DataType>
  Chebyshev1BandPassCoefficients<DataType>::Chebyshev1BandPassCoefficients(int nb_channels)
  :Parent(nb_channels, nb_channels), cut_frequencies(0, 0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev1BandPassCoefficients<DataType_>::set_cut_frequencies(std::pair<DataType_, DataType_> cut_frequencies)
  {
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev1BandPassCoefficients<DataType_>::set_cut_frequencies(DataType_ f0, DataType_ f1)
  {
    this->cut_frequencies = std::make_pair(f0, f1);
    setup();
  }

  template <typename DataType_>
  std::pair<DataType_, DataType_> Chebyshev1BandPassCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev1BandPassCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev1BandPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev1BandPassCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  void Chebyshev1BandPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_bp_chebyshev1_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev1BandStopCoefficients<DataType>::Chebyshev1BandStopCoefficients(int nb_channels)
  :Parent(nb_channels, nb_channels), cut_frequencies(0, 0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev1BandStopCoefficients<DataType_>::set_cut_frequencies(std::pair<DataType_, DataType_> cut_frequencies)
  {
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev1BandStopCoefficients<DataType_>::set_cut_frequencies(DataType_ f0, DataType_ f1)
  {
    this->cut_frequencies = std::make_pair(f0, f1);
    setup();
  }

  template <typename DataType_>
  std::pair<DataType_, DataType_> Chebyshev1BandStopCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev1BandStopCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev1BandStopCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev1BandStopCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  void Chebyshev1BandStopCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_bs_chebyshev1_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template class Chebyshev1LowPassCoefficients<float>;
  template class Chebyshev1LowPassCoefficients<double>;
  template class Chebyshev1HighPassCoefficients<float>;
  template class Chebyshev1HighPassCoefficients<double>;
  template class Chebyshev1BandPassCoefficients<float>;
  template class Chebyshev1BandPassCoefficients<double>;
  template class Chebyshev1BandStopCoefficients<float>;
  template class Chebyshev1BandStopCoefficients<double>;

  template class IIRFilter<Chebyshev1LowPassCoefficients<float> >;
  template class IIRFilter<Chebyshev1LowPassCoefficients<double> >;
  template class IIRFilter<Chebyshev1HighPassCoefficients<float> >;
  template class IIRFilter<Chebyshev1HighPassCoefficients<double> >;
  template class IIRFilter<Chebyshev1BandPassCoefficients<float> >;
  template class IIRFilter<Chebyshev1BandPassCoefficients<double> >;
  template class IIRFilter<Chebyshev1BandStopCoefficients<float> >;
  template class IIRFilter<Chebyshev1BandStopCoefficients<double> >;

}