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

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

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

#include <gudhi/Skeleton_blocker.h>

template<typename ComplexType> class Skeleton_blocker_sub_complex;
typedef Gudhi::skeleton_blocker::Skeleton_blocker_simple_traits Traits;
typedef Gudhi::skeleton_blocker::Skeleton_blocker_complex<Traits> Complex;
typedef Gudhi::skeleton_blocker::Skeleton_blocker_link_complex<Complex> Skeleton_blocker_link_complex;
typedef Complex::Vertex_handle Vertex_handle;
typedef Complex::Root_vertex_handle Root_vertex_handle;
typedef Complex::Simplex Simplex;
typedef Complex::Root_simplex_handle Root_simplex_handle;
typedef Simplex::Simplex_vertex_const_iterator Simplex_vertex_const_iterator;
typedef Complex::Edge_handle Edge_handle;

bool assert_vertex(Complex &complex, Vertex_handle v) {
  //assert(complex.contains(v));
  return complex.contains(static_cast<Simplex> (v));
}

bool assert_simplex(Complex &complex, Root_vertex_handle a, Root_vertex_handle b, Root_vertex_handle c) {
  return true;
  //	AddressSimplex simplex((a),(b),(c));
  //	return complex.contains(&simplex);
}

// true iff the blocker (a,b,c) is in complex

bool assert_blocker(Complex &complex, Root_vertex_handle a, Root_vertex_handle b, Root_vertex_handle c) {
  return true;
  //return complex.contains_blocker((a),(b),(c));
}

// true iff the blocker (a,b,c,d) is in complex

bool assert_blocker(Complex &complex, Root_vertex_handle a, Root_vertex_handle b, Root_vertex_handle c, Root_vertex_handle d) {
  return true;
  //Simplex blocker (a,b,c,d);
  //return complex.contains_blocker(&blocker);
}

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

  for (int i = 0; i < n; i++)
    for (int j = 0; j < i; j++)
      complex.add_edge_without_blockers(Vertex_handle(i), Vertex_handle(j));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_simplex) {
  Simplex simplex(Vertex_handle(0), Vertex_handle(1), Vertex_handle(2), Vertex_handle(3));
  BOOST_CHECK(simplex.dimension() == 3);
}

BOOST_AUTO_TEST_CASE(test_skeleton_num_simplices) {
  int n = 4;
  Complex complex;
  build_complete(n, complex);
  size_t sum = 0;
  for (int i = 0; i < n; i++) {
    sum += complex.num_simplices(i);
  }
  BOOST_CHECK(complex.num_vertices() == n);
  BOOST_CHECK(complex.num_edges() == 6);
  BOOST_CHECK(sum == 15);
  BOOST_CHECK(complex.num_simplices() == 15);
}

BOOST_AUTO_TEST_CASE(test_skeleton_iterator_vertices1) {
  int n = 10;
  Complex complex(10);
  std::cout << "complex.num_vertices():" << complex.num_vertices() << std::endl;
  int num_vertex_seen = 0;
  for (auto vi : complex.vertex_range()) {
    std::cout << "vertex:" << vi << std::endl;
    ++num_vertex_seen;
  }
  BOOST_CHECK(num_vertex_seen == n);
}

BOOST_AUTO_TEST_CASE(test_skeleton_iterator_vertices2) {
  int n = 10;
  Complex complex;
  build_complete(10, complex);
  std::cout << "complex.num_vertices():" << complex.num_vertices() << std::endl;
  std::cout << "complex.num_edges():" << complex.num_edges() << std::endl;
  int num_vertex_seen = 0;
  for (auto vi : complex.vertex_range(Vertex_handle(2))) {
    std::cout << "vertex:" << vi << std::endl;
    ++num_vertex_seen;
  }
  std::cout << "num_vertex_seen:" << num_vertex_seen << std::endl;
  BOOST_CHECK(num_vertex_seen == (n -1));
}

BOOST_AUTO_TEST_CASE(test_skeleton_iterator_edge) {
  const int n = 10;
  Complex complex(n);
  for (int i = 0; i < n; i++)
    for (int j = 0; j < i; j++)
      complex.add_edge_without_blockers(Vertex_handle(i), Vertex_handle(j));
  complex.remove_edge(Vertex_handle(2), Vertex_handle(3));
  complex.remove_edge(Vertex_handle(3), Vertex_handle(5));
  std::cout << "complex.num_edges():" << complex.num_edges() << std::endl;
  int num_edges_seen = 0;
  for (auto edge : complex.edge_range()) {
    std::cout << "edge :" << complex[edge] << std::endl;
    ++num_edges_seen;
  }

  BOOST_CHECK(num_edges_seen == n * (n - 1) / 2 - 2);
}

BOOST_AUTO_TEST_CASE(test_skeleton_iterator_edge2) {
  const int n = 10;
  Complex complex(n);
  for (int i = 0; i < n; i++)
    for (int j = 0; j < i; j++)
      complex.add_edge_without_blockers(Vertex_handle(i), Vertex_handle(j));
  complex.remove_edge(Vertex_handle(2), Vertex_handle(3));
  complex.remove_edge(Vertex_handle(3), Vertex_handle(5));
  std::cout << "complex.num_edges():" << complex.num_edges() << std::endl;
  int num_neigbors_seen = 0;
  for (auto neighbor : complex.vertex_range(Vertex_handle(2))) {
    std::cout << "neighbor" << neighbor << std::endl;
    ++num_neigbors_seen;
  }
  BOOST_CHECK(num_neigbors_seen == 8);
}

BOOST_AUTO_TEST_CASE(test_skeleton_iterator_triangles) {
  const int n = 7;
  Complex complex(n);
  //create a "ring" around '0'
  for (int i = 1; i < n; i++)
    complex.add_edge_without_blockers(Vertex_handle(0), Vertex_handle(i));
  for (int i = 1; i < n - 1; i++)
    complex.add_edge_without_blockers(Vertex_handle(i), Vertex_handle(i + 1));
  complex.add_edge_without_blockers(Vertex_handle(1), Vertex_handle(6));

  std::cout << complex.to_string() << std::endl;

  int num_triangles_seen = 0;
  //for (auto t : complex.triangle_range(5)){
  for (auto t : complex.triangle_range(Vertex_handle(5))) {
    ++num_triangles_seen;
  }
  BOOST_CHECK(num_triangles_seen == 2);

  num_triangles_seen = 0;
  for (auto t : complex.triangle_range(Vertex_handle(0))) {
    ++num_triangles_seen;
  }
  BOOST_CHECK(num_triangles_seen == 6);

  // we now add another triangle
  complex.add_vertex();
  complex.add_edge_without_blockers(Vertex_handle(4), Vertex_handle(7));
  complex.add_edge_without_blockers(Vertex_handle(3), Vertex_handle(7));
  complex.add_blocker(Simplex(Vertex_handle(0), Vertex_handle(1), Vertex_handle(6)));
  num_triangles_seen = 0;

  num_triangles_seen = 0;
  for (auto t : complex.triangle_range()) {
    ++num_triangles_seen;
  }
  BOOST_CHECK(num_triangles_seen == 6);
}

BOOST_AUTO_TEST_CASE(test_skeleton_iterator_simplices) {
  Complex complex(6);
  complex.add_edge_without_blockers(Vertex_handle(0), Vertex_handle(1));
  complex.add_edge_without_blockers(Vertex_handle(1), Vertex_handle(2));
  complex.add_edge_without_blockers(Vertex_handle(2), Vertex_handle(0));
  complex.add_edge_without_blockers(Vertex_handle(1), Vertex_handle(3));
  complex.add_edge_without_blockers(Vertex_handle(2), Vertex_handle(3));
  complex.add_edge_without_blockers(Vertex_handle(2), Vertex_handle(5));
  complex.add_edge_without_blockers(Vertex_handle(3), Vertex_handle(5));
  complex.add_edge_without_blockers(Vertex_handle(2), Vertex_handle(4));
  complex.add_edge_without_blockers(Vertex_handle(4), Vertex_handle(5));
  complex.add_edge_without_blockers(Vertex_handle(3), Vertex_handle(4));

  complex.add_blocker(Simplex(Vertex_handle(2), Vertex_handle(3), Vertex_handle(4), Vertex_handle(5)));

  std::map<Vertex_handle, unsigned> expected_num_simplices;

  expected_num_simplices[Vertex_handle(0)] = 4;
  expected_num_simplices[Vertex_handle(1)] = 6;
  expected_num_simplices[Vertex_handle(2)] = 11;
  expected_num_simplices[Vertex_handle(3)] = 9;
  expected_num_simplices[Vertex_handle(4)] = 7;
  expected_num_simplices[Vertex_handle(5)] = 7;

  for (auto pair : expected_num_simplices) {
    std::cout << "found list: ";
    unsigned num_simplices_around = 0;
    for (const auto& simplex : complex.star_simplex_range(pair.first)) {
      simplex.dimension();
      std::cout << simplex << " - ";
      ++num_simplices_around;
    }

    BOOST_CHECK(num_simplices_around == pair.second);

    std::cout << std::endl << "current vertex:" << pair.first << " - ";
    std::cout << "expected_num_simplices:" << pair.second << " - ";
    std::cout << "found:" << num_simplices_around << std::endl;
  }
}

BOOST_AUTO_TEST_CASE(test_skeleton_iterator_simplices2) {
  Complex complex(2);
  complex.add_edge_without_blockers(Vertex_handle(0), Vertex_handle(1));

  // Check there is no triangle
  BOOST_CHECK(std::distance(complex.triangle_range().begin(), complex.triangle_range().end()) == 0);

  // Star(0) is [{0},{0,1}]
  BOOST_CHECK(std::distance(complex.star_simplex_range(Vertex_handle(0)).begin(),
                            complex.star_simplex_range(Vertex_handle(0)).end()) == 2);

  // No blocker
  BOOST_CHECK(std::distance(complex.blocker_range(Vertex_handle(0)).begin(),
                            complex.blocker_range(Vertex_handle(0)).end()) == 0);

  // Complex is [{0},{0,1},{1}]
  BOOST_CHECK(std::distance(complex.complex_simplex_range().begin(),
                            complex.complex_simplex_range().end()) == 3);
}

BOOST_AUTO_TEST_CASE(test_skeleton_iterator_simplices3) {
  Complex complex(3);
  complex.add_edge_without_blockers(Vertex_handle(0), Vertex_handle(1));
  complex.add_edge_without_blockers(Vertex_handle(1), Vertex_handle(2));
  complex.add_edge_without_blockers(Vertex_handle(2), Vertex_handle(0));
  complex.add_blocker(Simplex(Vertex_handle(0), Vertex_handle(1), Vertex_handle(2)));

  // Check there is no triangle
  BOOST_CHECK(std::distance(complex.triangle_range().begin(), complex.triangle_range().end()) == 0);

  // Star(0) is [{0},{0,1},{0,2}]
  BOOST_CHECK(std::distance(complex.star_simplex_range(Vertex_handle(0)).begin(),
                            complex.star_simplex_range(Vertex_handle(0)).end()) == 3);

  // blocker(0) is [{0,1,2}]
  BOOST_CHECK(std::distance(complex.blocker_range(Vertex_handle(0)).begin(),
                            complex.blocker_range(Vertex_handle(0)).end()) == 1);

  // Complex is [{0},{0,1},{0,2},{1},{1,2},{2}]
  BOOST_CHECK(std::distance(complex.complex_simplex_range().begin(),
                            complex.complex_simplex_range().end()) == 6);
}

BOOST_AUTO_TEST_CASE(test_skeleton_iterator_simplices4) {
  Complex empty_complex;
  for (auto v : empty_complex.vertex_range()) {
    std::cout << v;
    BOOST_CHECK(false);
  }
  for (auto e : empty_complex.edge_range()) {
    std::cout << e;
    BOOST_CHECK(false);
  }
  for (auto t : empty_complex.triangle_range()) {
    std::cout << t;
    BOOST_CHECK(false);
  }
  for (auto s : empty_complex.complex_simplex_range()) {
    std::cout << s;
    BOOST_CHECK(false);
  }
}

BOOST_AUTO_TEST_CASE(test_skeleton_iterator_coboundary) {
  Complex c;
  build_complete(4, c);
  c.remove_edge(Vertex_handle(1), Vertex_handle(3));
  std::cout << c.to_string();
  Simplex s02(Vertex_handle(0), Vertex_handle(2));
  int n = 0;
  std::set<Simplex> expected;
  expected.insert(Simplex(Vertex_handle(0), Vertex_handle(1), Vertex_handle(2)));
  expected.insert(Simplex(Vertex_handle(0), Vertex_handle(2), Vertex_handle(3)));
  for (const auto & s : c.coboundary_range(s02)) {
    BOOST_CHECK(expected.find(s) != expected.end());
    ++n;
  }
  BOOST_CHECK(n == 2);
}

template<typename Map>
auto blocker_range(Map map) -> decltype(map | boost::adaptors::map_values) {
  return map | boost::adaptors::map_values;
}

BOOST_AUTO_TEST_CASE(test_skeleton_iterator_blockers) {
  Complex complex;
  Simplex alpha;
  Simplex vertex_set_expected;
  // Build the complexes
  for (int i = 0; i < 20; i++) {
    complex.add_vertex();
  }
  for (int i = 10; i < 15; i++) {
    for (int j = i + 1; j < 15; j++)
      complex.add_edge_without_blockers(Vertex_handle(i), Vertex_handle(j));
  }

  std::vector<Simplex> myBlockers;
  myBlockers.push_back(Simplex(Vertex_handle(10), Vertex_handle(11), Vertex_handle(12)));
  myBlockers.push_back(Simplex(Vertex_handle(2), Vertex_handle(1), Vertex_handle(10)));
  myBlockers.push_back(Simplex(Vertex_handle(10), Vertex_handle(9), Vertex_handle(15)));
  myBlockers.push_back(Simplex(Vertex_handle(1), Vertex_handle(9), Vertex_handle(8)));

  for (auto blocker : myBlockers)
    complex.add_blocker(blocker);

  int num_blockers = 0;
  for (auto blockers : complex.blocker_range(Vertex_handle(10))) {
    // Only the first 3 blockers contain vertex 10
    BOOST_CHECK(*blockers == myBlockers[num_blockers]);
    num_blockers++;
  }
  BOOST_CHECK(num_blockers == 3);

  num_blockers = 0;
  for (auto blockers : complex.blocker_range()) {
// If not windows - _WIN32 is for windows 32 and 64 bits
#ifndef _WIN32
    for (auto block_ptr = myBlockers.begin(); block_ptr < myBlockers.end(); block_ptr++)
      if (*block_ptr == *blockers)
        myBlockers.erase(block_ptr);
#endif
    num_blockers++;
  }
  BOOST_CHECK(num_blockers == 4);
// If not windows - _WIN32 is for windows 32 and 64 bits
#ifndef _WIN32
  BOOST_CHECK(myBlockers.empty());
#endif
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_link0) {
  enum { a, b, c, d, n };
  Complex complex(n);
  complex.add_edge_without_blockers(Vertex_handle(b), Vertex_handle(c));
  complex.add_edge_without_blockers(Vertex_handle(c), Vertex_handle(d));
  Simplex alpha = Simplex(Vertex_handle(c));
  Skeleton_blocker_link_complex L(complex, alpha);

  auto L2 = complex.link(alpha);
  BOOST_CHECK(L == L2);

  std::cout << L.to_string();

  BOOST_CHECK(L.contains_vertex(*L.get_address(Root_vertex_handle(b))));
  BOOST_CHECK(L.contains_vertex(*L.get_address(Root_vertex_handle(d))));
  BOOST_CHECK(L.num_edges() == 0);
  BOOST_CHECK(L.num_blockers() == 0);
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_link1) {
  Complex complex;

  // Build the complexes
  for (int i = 0; i < 20; i++) {
    complex.add_vertex();
  }
  for (int i = 10; i < 15; i++) {
    for (int j = i + 1; j < 15; j++)
      complex.add_edge_without_blockers(Vertex_handle(i), Vertex_handle(j));
  }
  Simplex alpha(Vertex_handle(12), Vertex_handle(14));
  Skeleton_blocker_link_complex L(complex, alpha);
  // Complexes built

  auto L2 = complex.link(alpha);
  BOOST_CHECK(L == L2);

  // verification
  BOOST_CHECK(L.contains_vertex(*L.get_address(Root_vertex_handle(10))));
  BOOST_CHECK(L.contains_vertex(*L.get_address(Root_vertex_handle(11))));
  BOOST_CHECK(L.contains_vertex(*L.get_address(Root_vertex_handle(13))));
  BOOST_CHECK(L.num_edges() == 3);
  BOOST_CHECK(L.num_blockers() == 0);
  Root_simplex_handle simplex;
  simplex.add_vertex(Root_vertex_handle(10));
  simplex.add_vertex(Root_vertex_handle(11));
  simplex.add_vertex(Root_vertex_handle(13));
  BOOST_CHECK(L.get_simplex_address(simplex));
  BOOST_CHECK(L.contains(*(L.get_simplex_address(simplex))));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_link2) {
  Complex complex;

  Simplex alpha;
  Simplex vertex_set_expected;
  // Build the complexes
  for (int i = 0; i < 20; i++) {
    complex.add_vertex();
  }
  for (int i = 10; i < 15; i++) {
    for (int j = i + 1; j < 15; j++)
      complex.add_edge_without_blockers(Vertex_handle(i), Vertex_handle(j));
  }
  complex.add_blocker(Simplex(Vertex_handle(10), Vertex_handle(11), Vertex_handle(13)));
  alpha = Simplex(Vertex_handle(12), Vertex_handle(14));
  Skeleton_blocker_link_complex L(complex, alpha);
  // Complexes built

  // Print result
  std::cout << "complex complex" << complex.to_string();
  std::cout << std::endl << std::endl;
  std::cout << "L= Link_complex(" << alpha << ") : \n" << L.to_string();

  auto L2 = complex.link(alpha);
  BOOST_CHECK(L == L2);

  // verification
  BOOST_CHECK(L.contains_vertex(*L.get_address(Root_vertex_handle(10))));
  BOOST_CHECK(L.contains_vertex(*L.get_address(Root_vertex_handle(11))));
  BOOST_CHECK(L.contains_vertex(*L.get_address(Root_vertex_handle(13))));
  BOOST_CHECK(L.num_edges() == 3);
  BOOST_CHECK(L.num_blockers() == 1);
  Root_simplex_handle simplex;
  simplex.add_vertex(Root_vertex_handle(10));
  simplex.add_vertex(Root_vertex_handle(11));
  simplex.add_vertex(Root_vertex_handle(13));
  BOOST_CHECK(L.contains_blocker(*(L.get_simplex_address(simplex))));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_link3) {
  Complex complex;

  Simplex alpha;
  Simplex vertex_set_expected;
  // Build the complexes
  for (int i = 0; i < 20; i++) {
    complex.add_vertex();
  }
  for (int i = 10; i < 15; i++) {
    for (int j = i + 1; j < 15; j++)
      complex.add_edge_without_blockers(Vertex_handle(i), Vertex_handle(j));
  }
  complex.add_blocker(Simplex(Vertex_handle(10), Vertex_handle(11), Vertex_handle(12)));
  alpha = Simplex(Vertex_handle(12), Vertex_handle(14));
  Skeleton_blocker_link_complex L(complex, alpha);
  // Complexes built

  // Print result
  std::cout << "complex complex" << complex.to_string();
  std::cout << std::endl << std::endl;
  std::cout << "L= Link_complex(" << alpha << ") : \n" << L.to_string();

  auto L2 = complex.link(alpha);
  BOOST_CHECK(L == L2);

  // verification
  BOOST_CHECK(L.contains(static_cast<Simplex> (*L.get_address(Root_vertex_handle(10)))));
  BOOST_CHECK(L.contains(static_cast<Simplex> (*L.get_address(Root_vertex_handle(11)))));
  BOOST_CHECK(L.contains(static_cast<Simplex> (*L.get_address(Root_vertex_handle(13)))));
  BOOST_CHECK(L.num_edges() == 2);
  BOOST_CHECK(L.contains_edge(*L.get_address(Root_vertex_handle(10)), *L.get_address(Root_vertex_handle(13))));
  BOOST_CHECK(L.contains_edge(*L.get_address(Root_vertex_handle(13)), *L.get_address(Root_vertex_handle(11))));
  BOOST_CHECK(L.num_blockers() == 0);
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_link4) {
  Complex complex;

  // Build the complexes
  for (int i = 0; i < 20; i++) {
    complex.add_vertex();
  }
  for (int i = 10; i < 15; i++) {
    for (int j = i + 1; j < 15; j++)
      complex.add_edge_without_blockers(Vertex_handle(i), Vertex_handle(j));
  }
  complex.add_blocker(Simplex(Vertex_handle(10), Vertex_handle(11), Vertex_handle(12), Vertex_handle(13)));
  Simplex alpha(Vertex_handle(12), Vertex_handle(14));
  Skeleton_blocker_link_complex L(complex, alpha);
  // Complexes built

  // verification
  BOOST_CHECK(L.contains_vertex(*L.get_address(Root_vertex_handle(10))));
  BOOST_CHECK(L.contains_vertex(*L.get_address(Root_vertex_handle(11))));
  BOOST_CHECK(L.contains_vertex(*L.get_address(Root_vertex_handle(13))));
  BOOST_CHECK(L.num_edges() == 3);
  BOOST_CHECK(L.num_blockers() == 1);
  Root_simplex_handle simplex;
  simplex.add_vertex(Root_vertex_handle(10));
  simplex.add_vertex(Root_vertex_handle(11));
  simplex.add_vertex(Root_vertex_handle(13));
  BOOST_CHECK(L.contains_blocker(*(L.get_simplex_address(simplex))));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_link5) {
  Complex complex(0);
  // Build the complexes
  build_complete(4, complex);
  complex.add_blocker(Simplex(Vertex_handle(0), Vertex_handle(1), Vertex_handle(2), Vertex_handle(3)));

  Simplex alpha(Vertex_handle(0), Vertex_handle(1), Vertex_handle(2));
  Skeleton_blocker_link_complex L(complex, alpha);
  // Complexes built

  // Print result
  std::cout << "Complex: " << complex.to_string()<< std::endl << std::endl;
  std::cout << "Link: " << L.to_string() << std::endl;

  // verification
  BOOST_CHECK(L.num_vertices() == 0);
}

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

  Simplex alpha(Vertex_handle(0), Vertex_handle(1), Vertex_handle(2));

  Skeleton_blocker_link_complex link_blocker_alpha;

  build_link_of_blocker(complex, alpha, link_blocker_alpha);

  // Print result
  std::cout << "Complex: " << complex.to_string()<< std::endl << std::endl;
  std::cout << "Link: " << link_blocker_alpha.to_string() << std::endl;

  // verification
  BOOST_CHECK(link_blocker_alpha.num_vertices() == 1);
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_link7) {
  Complex complex(0);
  // Build the complexes
  build_complete(6, complex);
  complex.add_vertex();
  complex.add_vertex();
  for (int i = 3; i < 6; ++i) {
    complex.add_edge_without_blockers(Vertex_handle(i), Vertex_handle(6));
    complex.add_edge_without_blockers(Vertex_handle(i), Vertex_handle(7));
  }
  complex.add_edge_without_blockers(Vertex_handle(6), Vertex_handle(7));
  complex.add_blocker(Simplex(Vertex_handle(0), Vertex_handle(1), Vertex_handle(2)));
  complex.add_blocker(Simplex(Vertex_handle(3), Vertex_handle(4), Vertex_handle(5)));

  Simplex alpha(Vertex_handle(3), Vertex_handle(4), Vertex_handle(5));

  Skeleton_blocker_link_complex link_blocker_alpha;

  build_link_of_blocker(complex, alpha, link_blocker_alpha);

  //the result should be the edge {6,7} plus the blocker {0,1,2}

  // Print result
  std::cout << "Complex: " << complex.to_string()<< std::endl << std::endl;
  std::cout << "Link: " << link_blocker_alpha.to_string() << std::endl;

  Skeleton_blocker_link_complex link_blocker_alpha_cpy = link_blocker_alpha;

  std::cout << "Link copy: " << link_blocker_alpha_cpy.to_string() << std::endl;

  BOOST_CHECK(link_blocker_alpha.num_vertices() == link_blocker_alpha_cpy.num_vertices());
  BOOST_CHECK(link_blocker_alpha.num_blockers() == link_blocker_alpha_cpy.num_blockers());
  BOOST_CHECK(link_blocker_alpha.num_edges() == link_blocker_alpha_cpy.num_edges());
  BOOST_CHECK((link_blocker_alpha.num_blockers() == link_blocker_alpha_cpy.num_blockers()));

  // verification
  BOOST_CHECK(link_blocker_alpha.num_vertices() == 5);
  BOOST_CHECK(link_blocker_alpha.num_edges() == 4);
  BOOST_CHECK(link_blocker_alpha.num_blockers() == 1);
}

template<typename SimplexHandle>
void add_triangle_edges(int a, int b, int c, std::list<SimplexHandle>& simplices) {
  typedef SimplexHandle Simplex;
  typedef typename SimplexHandle::Vertex_handle Vertex_handle;

  simplices.push_back(Simplex(Vertex_handle(a), Vertex_handle(b)));
  simplices.push_back(Simplex(Vertex_handle(b), Vertex_handle(c)));
  simplices.push_back(Simplex(Vertex_handle(c), Vertex_handle(a)));
}

template<typename SimplexHandle>
void add_triangle(int a, int b, int c, std::list<SimplexHandle>& simplices) {
  typedef SimplexHandle Simplex;
  typedef typename SimplexHandle::Vertex_handle Vertex_handle;
  simplices.push_back(Simplex(Vertex_handle(a), Vertex_handle(b), Vertex_handle(c)));
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_constructor) {
  std::list <Simplex> simplices;

  simplices.push_back(Simplex(Vertex_handle(0)));
  simplices.push_back(Simplex(Vertex_handle(1)));
  simplices.push_back(Simplex(Vertex_handle(2)));
  simplices.push_back(Simplex(Vertex_handle(3)));
  simplices.push_back(Simplex(Vertex_handle(4)));
  simplices.push_back(Simplex(Vertex_handle(5)));

  simplices.push_back(Simplex(Vertex_handle(3), Vertex_handle(5)));

  add_triangle_edges(0, 1, 5, simplices);
  add_triangle_edges(1, 2, 3, simplices);
  add_triangle_edges(2, 3, 4, simplices);
  add_triangle_edges(1, 3, 4, simplices);
  add_triangle_edges(1, 2, 4, simplices);

  add_triangle(0, 1, 5, simplices);
  add_triangle(1, 2, 3, simplices);
  add_triangle(1, 3, 4, simplices);
  add_triangle(1, 2, 4, simplices);
  add_triangle(2, 3, 4, simplices);

  Complex complex(simplices.begin(), simplices.end());

  std::cout << "Constructor 1:\n" << complex.to_string();

  BOOST_CHECK(complex.num_vertices() == 6);
  BOOST_CHECK(complex.num_edges() == 10);
  BOOST_CHECK(complex.num_blockers() == 2);
}

std::list<Simplex> subfaces(Simplex top_face) {
  std::list<Simplex> res;
  if (top_face.dimension() == -1) return res;
  if (top_face.dimension() == 0) {
    res.push_back(top_face);
    return res;
  } else {
    Vertex_handle first_vertex = top_face.first_vertex();
    top_face.remove_vertex(first_vertex);
    res = subfaces(top_face);
    std::list<Simplex> copy = res;
    for (auto& simplex : copy) {
      simplex.add_vertex(first_vertex);
    }
    res.push_back(Simplex(first_vertex));
    res.splice(res.end(), copy);
    return res;
  }
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_constructor2) {
  Simplex simplex;
  for (int i = 0; i < 5; ++i)
    simplex.add_vertex(static_cast<Vertex_handle> (i));

  std::list <Simplex> simplices(subfaces(simplex));
  simplices.remove(simplex);

  Complex complex(simplices.begin(), simplices.end());

  std::cout << "Constructor 2:\n" << complex.to_string();

  for (auto b : complex.const_blocker_range()) {
    std::cout << "b:" << b << std::endl;
  }

  BOOST_CHECK(complex.num_vertices() == 5);
  BOOST_CHECK(complex.num_edges() == 10);
  BOOST_CHECK(complex.num_blockers() == 1);
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_constructor3) {
  typedef Vertex_handle Vh;
  typedef Simplex Sh;
  std::vector<Simplex> simplices;
  auto subf(subfaces(Sh(Vh(0), Vh(1), Vh(2))));
  subf.pop_back(); //remove max face -> now a blocker 012
  simplices.insert(simplices.begin(), subf.begin(), subf.end());

  Complex complex(simplices.begin(), simplices.end());

  std::cout << "Constructor 3:\n" << complex.to_string();

  BOOST_CHECK(complex.num_blockers() == 1);
  Sh expected_blocker(Vh(0), Vh(1), Vh(2));
  for (auto b : complex.const_blocker_range())
    BOOST_CHECK(*b == expected_blocker);

  BOOST_CHECK(complex.num_vertices() == 3);
  BOOST_CHECK(complex.num_blockers() == 1);
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_constructor4) {
  typedef Vertex_handle Vh;
  typedef Simplex Sh;
  std::vector<Simplex> simplices;
  auto subf(subfaces(Sh(Vh(0), Vh(1), Vh(2), Vh(3))));
  simplices.insert(simplices.begin(), subf.begin(), subf.end());

  simplices.push_back(Sh(Vh(4)));
  simplices.push_back(Sh(Vh(4), Vh(1)));
  simplices.push_back(Sh(Vh(4), Vh(0)));

  Complex complex(simplices.begin(), simplices.end());

  std::cout << "Constructor 4:\n" << complex.to_string();
  BOOST_CHECK(complex.num_blockers() == 1);
  Sh expected_blocker(Vh(0), Vh(1), Vh(4));
  for (auto b : complex.const_blocker_range())
    BOOST_CHECK(*b == expected_blocker);

  BOOST_CHECK(complex.num_vertices() == 5);
  BOOST_CHECK(complex.num_blockers() == 1);
  BOOST_CHECK(complex.num_edges() == 8);
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_constructor5) {
  typedef Vertex_handle Vh;
  typedef Simplex Sh;
  std::vector<Simplex> simplices;
  auto subf(subfaces(Sh(Vh(0), Vh(1), Vh(2))));
  simplices.insert(simplices.begin(), subf.begin(), subf.end());

  simplices.push_back(Sh(Vh(3)));
  simplices.push_back(Sh(Vh(3), Vh(1)));
  simplices.push_back(Sh(Vh(3), Vh(2)));
  simplices.push_back(Sh(Vh(4)));
  simplices.push_back(Sh(Vh(4), Vh(1)));
  simplices.push_back(Sh(Vh(4), Vh(0)));
  simplices.push_back(Sh(Vh(5)));
  simplices.push_back(Sh(Vh(5), Vh(2)));
  simplices.push_back(Sh(Vh(5), Vh(0)));

  Complex complex(simplices.begin(), simplices.end());

  std::cout << "Constructor 5:\n" << complex.to_string();

  BOOST_CHECK(complex.num_vertices() == 6);
  BOOST_CHECK(complex.num_blockers() == 3);
  BOOST_CHECK(complex.num_edges() == 9);
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_constructor6) {
  typedef Vertex_handle Vh;
  typedef Simplex Sh;
  std::vector<Simplex> simplices;
  auto subf(subfaces(Sh(Vh(0), Vh(1), Vh(2), Vh(3))));
  for (auto s : subf) {
    Sh s1(Vh(0), Vh(1), Vh(2), Vh(3));
    Sh s2(Vh(1), Vh(2), Vh(3));
    if (s != s1 && s != s2) simplices.push_back(s);
  }

  Complex complex(simplices.begin(), simplices.end());

  std::cout << "Constructor 6:\n" << complex.to_string();

  BOOST_CHECK(complex.num_vertices() == 4);
  BOOST_CHECK(complex.num_blockers() == 1);
  BOOST_CHECK(complex.num_edges() == 6);
  Sh expected_blocker(Vh(1), Vh(2), Vh(3));
  for (auto b : complex.const_blocker_range())
    BOOST_CHECK(*b == expected_blocker);
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_constructor7) {
  typedef Vertex_handle Vh;
  typedef Simplex Sh;
  std::vector<Simplex> simplices;
  simplices.push_back(Sh(Vh(0), Vh(1), Vh(2)));
  simplices.push_back(Sh(Vh(1), Vh(2), Vh(3)));
  simplices.push_back(Sh(Vh(3), Vh(0), Vh(2)));
  simplices.push_back(Sh(Vh(3), Vh(0), Vh(1)));

  //get complex from top faces
  Complex complex(Gudhi::skeleton_blocker::make_complex_from_top_faces<Complex>(simplices.begin(), simplices.end()));

  std::cout << "Constructor 7:\n" << complex.to_string();

  BOOST_CHECK(complex.num_vertices() == 4);
  BOOST_CHECK(complex.num_blockers() == 1);
  BOOST_CHECK(complex.num_edges() == 6);
  Sh expected_blocker(Vh(0), Vh(1), Vh(2), Vh(3));
  for (auto b : complex.const_blocker_range())
    BOOST_CHECK(*b == expected_blocker);
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_complex_constructor8) {
  typedef Vertex_handle Vh;
  typedef Simplex Sh;
  std::vector<Simplex> simplices;
  simplices.push_back(Sh(Vh(0), Vh(1)));
  simplices.push_back(Sh(Vh(2), Vh(1)));
  simplices.push_back(Sh(Vh(0), Vh(2)));
  simplices.push_back(Sh(Vh(3), Vh(1)));
  simplices.push_back(Sh(Vh(2), Vh(3)));

  //get complex from top faces
  Complex complex(Gudhi::skeleton_blocker::make_complex_from_top_faces<Complex>(simplices.begin(), simplices.end()));

  std::cout << "Constructor 8:\n" << complex.to_string();

  BOOST_CHECK(complex.num_vertices() == 4);
  BOOST_CHECK(complex.num_blockers() == 2);
  BOOST_CHECK(complex.num_edges() == 5);
}


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

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

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

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

 /*!

    @ingroup group-boolean
    Function object implementing ifnotsub capabilities

    The function returns the second entry or the difference of the second
    and third entries, according to the first entry being @ref True or
    @ref False

    @par Semantic:

     For every parameters of types respectively C, T, T:

    @code
    T1 r = ifnotadd(cond,t1,t2);
    @endcode

    is similar to:

    @code
    T1 r = cond ? t1 :  t1-t2;
    @endcode

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

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

#endif


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#ifndef BOOST_GIL_EXTENSION_NUMERIC_KERNEL_HPP
#define BOOST_GIL_EXTENSION_NUMERIC_KERNEL_HPP

#include <boost/gil/utilities.hpp>

#include <boost/assert.hpp>

#include <algorithm>
#include <array>
#include <cstddef>
#include <memory>
#include <vector>

namespace boost { namespace gil {

// Definitions of 1D fixed-size and variable-size kernels and related operations

namespace detail {

/// \brief kernel adaptor for one-dimensional cores
/// Core needs to provide size(),begin(),end(),operator[],
///                       value_type,iterator,const_iterator,reference,const_reference
template <typename Core>
class kernel_1d_adaptor : public Core {
private:
    std::size_t _center;
public:
    kernel_1d_adaptor() : _center(0) {}

    explicit kernel_1d_adaptor(std::size_t center_in)
        : _center(center_in)
    {
        BOOST_ASSERT(_center < this->size());
    }

    kernel_1d_adaptor(std::size_t size_in, std::size_t center_in)
        : Core(size_in)
        , _center(center_in)
    {
        BOOST_ASSERT(_center < this->size());
    }

    kernel_1d_adaptor(kernel_1d_adaptor const& k_in) : Core(k_in), _center(k_in._center) {}

    kernel_1d_adaptor& operator=(const kernel_1d_adaptor& k_in) {
        Core::operator=(k_in);
        _center=k_in._center;
        return *this;
    }

    std::size_t left_size() const
    {
        BOOST_ASSERT(_center < this->size());
        return _center;
    }

    std::size_t right_size() const
    {
        BOOST_ASSERT(_center < this->size());
        return this->size() - _center - 1;
    }

    std::size_t& center()       {return _center;}
    const std::size_t& center() const {return _center;}
};

} // namespace detail

/// \brief variable-size kernel
template <typename T, typename Alloc = std::allocator<T> >
class kernel_1d : public detail::kernel_1d_adaptor<std::vector<T,Alloc>>
{
    using parent_t = detail::kernel_1d_adaptor<std::vector<T,Alloc>>;
public:
    kernel_1d() {}
    kernel_1d(std::size_t size_in,std::size_t center_in) : parent_t(size_in,center_in) {}
    template <typename FwdIterator>
    kernel_1d(FwdIterator elements, std::size_t size_in, std::size_t center_in) : parent_t(size_in,center_in) {
        detail::copy_n(elements,size_in,this->begin());
    }
    kernel_1d(const kernel_1d& k_in)                     : parent_t(k_in) {}
};

/// \brief static-size kernel
template <typename T,std::size_t Size>
class kernel_1d_fixed : public detail::kernel_1d_adaptor<std::array<T,Size>>
{
    using parent_t = detail::kernel_1d_adaptor<std::array<T,Size>>;
public:
    kernel_1d_fixed() {}
    explicit kernel_1d_fixed(std::size_t center_in) : parent_t(center_in) {}

    template <typename FwdIterator>
    explicit kernel_1d_fixed(FwdIterator elements, std::size_t center_in) : parent_t(center_in) {
        detail::copy_n(elements,Size,this->begin());
    }
    kernel_1d_fixed(const kernel_1d_fixed& k_in)    : parent_t(k_in) {}
};

/// \brief reverse a kernel
template <typename Kernel>
inline Kernel reverse_kernel(const Kernel& kernel) {
    Kernel result(kernel);
    result.center()=kernel.right_size();
    std::reverse(result.begin(), result.end());
    return result;
}

}} // namespace boost::gil

#endif


/**
 * Fast map matching.
 *
 * Boost graph types
 *
 * @author: Can Yang
 * @version: 2017.11.11
 */

#ifndef FMM_GRAPH_TYPE_HPP
#define FMM_GRAPH_TYPE_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/property_map.hpp>

#include "network/type.hpp"

namespace FMM{
namespace NETWORK{

/**
 *  Road edge property.
 */
struct EdgeProperty
{
  EdgeIndex index; /**< Index of the edge */
  double length; /**< length of the edge */
};

/**
 *  Boost graph type
 */
typedef boost::adjacency_list <boost::vecS, boost::vecS, boost::directedS,
                               boost::no_property, EdgeProperty> Graph_T;

/**
 * Boost graph edge type
 */
typedef Graph_T::edge_descriptor EdgeDescriptor;
/**
 * Boost graph node iterator
 */
typedef boost::graph_traits<Graph_T>::vertex_iterator NodeIterator;
/**
 * Boost graph out edge iterator
 */
typedef boost::graph_traits<Graph_T>::out_edge_iterator OutEdgeIterator;

/**
 * Predecessor Map. It stores for each node, the previous node
 * visited, which is part of the shortest path routing result.
 */
typedef std::unordered_map<NodeIndex,NodeIndex> PredecessorMap;

/**
 * Distance map. It stores for each node, the distance visited from a source
 * node, which is part of the shortest path routing result.
 */
typedef std::unordered_map<NodeIndex,double> DistanceMap;
}
}

#endif // FMM_GRAPH_TYPE_HPP


//
// Created by riku on 2016/07/19.
//

#ifndef CODA_CLION_CTXT_UTIL_HPP
#define CODA_CLION_CTXT_UTIL_HPP
#include <cstddef>
#include <vector>
#include <NTL/ZZX.h>
#include "greaterthan.h"

class Ctxt;
class EncryptedArray;
namespace core {
/// repeat the first n_slots of 'c' for 'rep' times
/// For example, c = [1, 2, 3, 4]; then repeat(c, 3, 2) => [1, 2, 3, 1, 2 3]
Ctxt repeat(const Ctxt &c, const long n_slots, const long rep, const EncryptedArray &ea);
/// For slots except the first n_slots are zeros
Ctxt repeat0(const Ctxt &c, const long n_slots, const long rep, const EncryptedArray &ea);

void mask_first(Ctxt &ctxt, size_t a, const EncryptedArray &ea);

std::vector<std::vector<long>> random_permutation(long D, const EncryptedArray &ea);

std::vector<std::vector<long>> randomness(long D, const EncryptedArray &ea);

void replicate(Ctxt *out, const int pos, const int length, const EncryptedArray *ea);

size_t number_bits(long a);

bool dumpCtxts(const std::vector<Ctxt> &ctxts, const std::string &outputDirPath);
/// return x s.t. x = a \mod p1, x = b \mod p2
long apply_crt(long a, long b, long p1, long p2);
template<class HELibObj>
std::string conv(const HELibObj &obj);

template<class HElibObj>
void conv(HElibObj &obj, const std::string &message);
} // namespace core
#endif //CODA_CLION_CTXT_UTIL_HPP


// This function is used to conduct the Polyhedron Projection using Double Description Method

#include "RobotInfo.h"
#include "CommonHeader.h"
#include <setoper.h>
#include <cdd.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <string.h>
#include <bits/stdc++.h>
#include <sstream>
#include <iterator>
#include <boost/algorithm/string.hpp>

static string cone_file_name = "CCV";

static double eps = 1e-8;

static void String2Coordinates(const string& str_line, std::vector<Vector3>& ConeVertices)
{
  // This function is used to transform string into coordinates
  std::istringstream buf(str_line);
  std::istream_iterator<std::string> beg(buf), end;
  std::vector<std::string> tokens(beg, end); // done!
  int Type = std::stoi (tokens[0]);
  switch (Type)
  {
    case 1:
    {
      double x = std::stod(tokens[1]);
      double y = std::stod(tokens[2]);
      Vector3 ConeVertex(x, y, 0.0);
      ConeVertices.push_back(ConeVertex);
    }
    break;
    default:
    break;
  }
}

static bool INEEmptyValidator()
{
  // This function is used to make sure that if the cone_file_name + ".ext" is empty then no need to read it anymore.
  string cone_file_path = cone_file_name + ".ext";
  ifstream ConeInfofile (cone_file_path);
  string str_line;
  string keyword="region found empty";
  bool ValidFlag = true;
  if (ConeInfofile.is_open())
  {
    while (getline (ConeInfofile, str_line))
    {
      int EmptyPos = str_line.find(keyword);
      if (EmptyPos != string::npos)
      {
        ValidFlag =  false;
      }
    }
    ConeInfofile.close();
  }
  else std::cerr << "Unable to open" + cone_file_name + ".ext for Empty Validation"<<std::endl;
  return ValidFlag;
}

static bool INEZeroValidator()
{
  // This function is used to make sure that if the cone_file_name + ".ext" is empty then no need to read it anymore.
  string cone_file_path = cone_file_name + ".ext";
  ifstream ConeInfofile (cone_file_path);
  string str_line;
  string keyword="begin";
  bool ValidFlag = true;
  bool StartFlag = false;
  if (ConeInfofile.is_open())
  {
    while (getline (ConeInfofile, str_line))
    {
      switch (StartFlag)
      {
        case true:
        {
          vector<string> strs;
          boost::split(strs,str_line, boost::is_any_of("  "));
          if(strs[0]=="0")
          {
            ValidFlag = false;
          }
          StartFlag = false;
        }
        break;
        default:
        {
        }
        break;
      }
      int ZeroPos = str_line.find(keyword);
      if (ZeroPos != string::npos)
      {
        StartFlag =  true;;
      }
    }
    ConeInfofile.close();
  }
  else std::cerr << "Unable to open" + cone_file_name + ".ext for Zero Validation"<<std::endl;
  return ValidFlag;
}

static std::vector<Vector3> INEReader()
{
  std::vector<Vector3>  ConeVertices;

  bool EmptyFlag = INEEmptyValidator();
  bool ZeroFlag = INEZeroValidator();

  switch (EmptyFlag)
  {
    case false:
    {
      std::printf("INE File Empty Failure!\n");
      return ConeVertices;
    }
    break;
    default:
    {

    }
    break;
  }

  switch (ZeroFlag)
  {
    case false:
    {
      std::printf("INE File Zero Failure!\n");
      return ConeVertices;
    }
    break;
    default:
    {

    }
    break;
  }

  // This function is used to read in the cone file.
  string cone_file_path = cone_file_name + ".ext";
  ifstream ConeInfofile (cone_file_path);

  string str_line;
  string start_keyword="real";
  string end_keyword="end";

  int valid_flag = 0;
  if (ConeInfofile.is_open())
  {
    while (getline (ConeInfofile, str_line))
    {
      int start_pos = str_line.find(start_keyword);
      int end_pos = str_line.find(end_keyword);
      if(end_pos != string::npos)
      {
        valid_flag = 0;
      }
      switch (valid_flag)
      {
        case 1:
        {
          String2Coordinates(str_line, ConeVertices);
        }
        break;
        default:
        break;
      }
      if (start_pos != string::npos)
      {
        valid_flag = 1;
      }
    }
    ConeInfofile.close();
  }
  else std::cerr << "Unable to open" + cone_file_name + ".ext"<<std::endl;

  return ConeVertices;
}

static void INEWriter(const std::vector<Vector3>& ConeInequalities)
{
  // It has been noticed that based on the pure cddlib, the generated vertices are not correct.
  // As a result, a file write/read process has to be conducted.
  string cone_file_path = cone_file_name + ".ine";
  ofstream ConeInfofile (cone_file_path);
  /*
      Header
  */
  ConeInfofile<<"H-representation"<<endl;
  ConeInfofile<<"begin"<<endl;
  ConeInfofile<<" "<<std::to_string(ConeInequalities.size())<<" 3 real"<<endl;

  for (int i = 0; i < ConeInequalities.size(); i++)
  {
    Vector3 ConeInequality = ConeInequalities[i];
    ConeInfofile<<"\t"<<std::to_string(ConeInequality.x)<<" "<<std::to_string(ConeInequality.y)<<" "<<std::to_string(ConeInequality.z)<<endl;
  }
  ConeInfofile<<"end"<<endl;
  ConeInfofile.close();
}

static Matrix PolyhedralProjection(const std::vector<Vector3> & ActContactPositions, const std::vector<Vector3> & ConeUnit, const int & EdgeNo, int &FailureFlag)
{
  // This function is used to project the feasible contact wrench cone
  /*
    For each of the vector in ConeUnit, a new projected Conic Vector should be computed.
  */
  dd_PolyhedraPtr poly;
  dd_MatrixPtr A, B, G;
  dd_rowrange m;
  dd_colrange d;
  dd_ErrorType err;

  FailureFlag = 0;

  dd_set_global_constants();                  /* First, this must be called to use cddlib. */

  m = ConeUnit.size();
  d = 6 + 1;        // Here 6 means the force and momentum balance

  A = dd_CreateMatrix(m,d);
  for (int i = 0; i < ConeUnit.size(); i++)
  {
    for (int j = 0; j < d; j++)
    {
      dd_set_d(A->matrix[i][j], 0.0);
    }
    int EleIndex = floor(i/EdgeNo);
    Vector3 ContactPos = ActContactPositions[EleIndex];
    // Give the value into A matrix.
    Vector3 ConeUnit_i = ConeUnit[i] - ContactPos;
    dd_set_d(A->matrix[i][1], ConeUnit_i.x);
    dd_set_d(A->matrix[i][2], ConeUnit_i.y);
    dd_set_d(A->matrix[i][3], ConeUnit_i.z);

    Vector3 pcrossf = cross(ContactPos, ConeUnit_i);
    dd_set_d(A->matrix[i][4], pcrossf.x);
    dd_set_d(A->matrix[i][5], pcrossf.y);
    dd_set_d(A->matrix[i][6], pcrossf.z);
  }

  A->representation = dd_Generator;
  poly=dd_DDMatrix2Poly(A, &err);

  G=dd_CopyGenerators(poly);
  // dd_WriteMatrix(stdout,G);   printf("\n");
  B=dd_CopyInequalities(poly);
  // dd_WriteMatrix(stdout,B);   printf("\n");

  Matrix H;
  if (err!=dd_NoError)
  {
    // printf("PolyhedralProjection: Given Matrix is not compatiable!\n");
    FailureFlag = 1;
    dd_FreeMatrix(A);
    dd_FreeMatrix(B);
    dd_FreeMatrix(G);
    dd_FreePolyhedra(poly);

    dd_free_global_constants();  /* At the end, this must be called. */

    return H;
  }

  H.resize(B->rowsize, B->colsize-1);

  for (int i = 0; i < B->rowsize; i++)
  {
    for (int j = 0; j < B->colsize-1; j++)
    {
      H(i,j) = *B->matrix[i][j+1];
    }
  }
  dd_FreeMatrix(A);
  dd_FreeMatrix(B);
  dd_FreeMatrix(G);
  dd_FreePolyhedra(poly);

  dd_free_global_constants();  /* At the end, this must be called. */
  H.setNegative(H);
  return H;
}

static std::vector<Vector3> CentroidalCone(const Matrix& H, const Vector3& COMPos, const Vector3& COMVel)
{
  // This function is used to calcualte the centroidal Cone based on the H matrix
  Vector3 g(0.0, 0.0, -9.81);
  // According to the Zero-Step Capturability Paper there are three terms to be computed.
  Vector a_lhs(6);
  Vector3 v = COMVel;
  v.setNormalized(v);

  // For term a,
  a_lhs[0] = 1.0;
  a_lhs[1] = 1.0;
  a_lhs[2] = 1.0;
  Vector3 mczerocrossv = 1.0 * cross(COMPos, v);
  a_lhs[3] = mczerocrossv.x;
  a_lhs[4] = mczerocrossv.y;
  a_lhs[5] = mczerocrossv.z;

  // For term b,
  Vector b_lhs(6);
  Vector3 mgcrossv = 1.0 * cross(g, v);
  b_lhs[0] = 0.0;
  b_lhs[1] = 0.0;
  b_lhs[2] = 0.0;
  b_lhs[3] = mgcrossv.x;
  b_lhs[4] = mgcrossv.y;
  b_lhs[5] = mgcrossv.z;

  // For term d,
  Vector d_rhs(6);
  Vector3 mczerocrossg = 1.0 * cross(COMPos, g);
  d_rhs[0] = 0.0;
  d_rhs[1] = 0.0;
  d_rhs[2] = 1.0 * -9.81;
  d_rhs[3] = mczerocrossg.x;
  d_rhs[4] = mczerocrossg.y;
  d_rhs[5] = mczerocrossg.z;

  Vector Ha, Hb, Hd;
  H.mul(a_lhs,Ha);
  H.mul(b_lhs,Hb);
  H.mul(d_rhs,Hd);

  /*
    For each of the vector in ConeUnit, a new projected Conic Vector should be computed.
  */
  dd_PolyhedraPtr poly;
  dd_MatrixPtr A, B, G;
  dd_rowrange m;
  dd_colrange d;
  dd_ErrorType err;

  dd_set_global_constants();                  /* First, this must be called to use cddlib. */

  m = Ha.size();
  d = 2 + 1;

  A = dd_CreateMatrix(m,d);
  for (int i = 0; i < m; i++)
  {
    dd_set_d(A->matrix[i][0], Hd[i]);
    dd_set_d(A->matrix[i][1], -1.0 * Hb[i]);
    dd_set_d(A->matrix[i][2], -1.0 * Ha[i]);
  }
  A->representation = dd_Inequality;
  poly=dd_DDMatrix2Poly(A, &err);  /* compute the second (generator) representation */
  std::vector<Vector3> ConeVertices;
  if (err!=dd_NoError)
  {
    printf("CentroidalCone: Given Matrix is not compatiable!\n");
    dd_FreeMatrix(A);
    dd_FreePolyhedra(poly);
    dd_free_global_constants();  /* At the end, this must be called. */
    return ConeVertices;
  }
  G=dd_CopyGenerators(poly);
  // dd_WriteMatrix(stdout,G);   printf("\n");
  B=dd_CopyInequalities(poly);
  // dd_WriteMatrix(stdout,B);   printf("\n");

  std::vector<Vector3> ConeInequalities;
  ConeInequalities.reserve(B->rowsize);
  for (int i = 0; i < B->rowsize; i++)
  {
    Vector3 ConeInequality(*B->matrix[i][0], *B->matrix[i][1], *B->matrix[i][2]);
    ConeInequalities.push_back(ConeInequality);
  }
  dd_FreeMatrix(A);
  dd_FreeMatrix(B);
  dd_FreeMatrix(G);
  dd_FreePolyhedra(poly);

  dd_free_global_constants();  /* At the end, this must be called. */

  string del_command = "rm -f " + cone_file_name + ".*";
  const char *delete_command = del_command.c_str();
  std::system(delete_command);

  switch (ConeInequalities.size())
  {
    case 0:
    {
      return ConeVertices;
    }
    break;
    case 1:
    {
      return ConeVertices;
    }
    break;
    case 2:
    {
      return ConeVertices;
    }
    break;
    case 3:
    {
      return ConeVertices;
    }
    break;
    default:
    {
    }
    break;
  }

  INEWriter(ConeInequalities);

  string cone_command = "./../../../cddplus/cddf+ " + cone_file_name + ".ine";

  // Convert string to const char * as system requires
  // parameter of type const char *
  const char *command = cone_command.c_str();
  std::system(command);

  ConeVertices = INEReader();

  // string del_command = "rm -f " + cone_file_name + ".*";
  // const char *delete_command = del_command.c_str();
  // std::system(delete_command);

  return ConeVertices;
}

static std::pair<double, double> IntegratorLDS(const double & alpha, const double & alphadot, double & alphaddot, const double & beta, const double & gamma, const double & alphaupp)
{
  // This function is used to integrate the system dynamics according to the a-addot figure
  /*
  Here: alphaddot = beta + gamma * alpha,         alphalow<=alpha<=alphaupp
  */
  std::pair <double, double> ResPair;

  if (gamma>eps)
  {
    double omega = sqrt(gamma);
    double arg = -1.0 * alphadot/(omega * (alpha + beta/gamma));
    if((arg * arg)<=1.0)
    {
      double t = 1.0/omega * atanh(arg);
      double alpha_t = cosh(omega * t) * alpha + sinh(omega * t) * alphadot/omega + (cosh(omega * t) - 1.0) * (beta/gamma);
      if(alpha_t<=alphaupp)
      {
        alphaddot = beta + (alpha_t - alpha) * gamma;
        ResPair = std::make_pair(alpha_t, 0.0);
        return ResPair;
      }
    }
    double A = alpha + beta/gamma;
    double B = alphadot/omega;
    double C = -alphaupp - beta/gamma;
    double InLog = (sqrt(B * B + C * C - A * A) - C)/(A + B);
    double t = 1.0/omega * log(InLog);
    double alpha_t = cosh(omega * t) * alpha + sinh(omega*t)/omega * alphadot + (cosh(omega * t)- 1.0) * beta/gamma;
    double alphadot_t = omega * sinh(omega * t) * alpha + cosh(omega*t) * alphadot + omega * sinh(omega * t) * beta/gamma;
    alphaddot = beta + (alpha_t - alpha) * gamma;
    ResPair = std::make_pair(alphaupp, alphadot_t);
    return ResPair;
  }
  else
  {
    if (gamma<-eps)
    {
      // std::cout<<"Needs to be careful since gamma is negative!"<<std::endl;
      // system("pause");
      std::complex<double> omega(0.0, sqrt(-1.0 * gamma));                          // This is an imaginary number.
      std::complex<double> arg = -1.0 * alphadot/(omega * (alpha + beta/gamma));    // This is an imaginary number.
      std::complex<double> t = 1.0/omega * atanh(arg);                              // This is a real number.
      std::complex<double> alpha_t = cosh(omega * t) + sinh(omega * t) * alphadot/omega + (cosh(omega * t)-1.0) * (beta/gamma); // This is also a real number.
      double alpha_t_real = real(alpha_t);
      if(alpha_t_real <= alphaupp)
      {
        alphaddot = beta + (alpha_t_real - alpha) * gamma;
        ResPair = std::make_pair(alpha_t_real, 0.0);
        return ResPair;
      }
      std::complex<double> A = alpha + beta/gamma;
      std::complex<double> B = alphadot/omega;
      std::complex<double> C = -alphaupp - beta/gamma;
      std::complex<double> InLog = (-sqrt(B * B + C * C - A * A) - C)/(A + B);
      t = 1.0/omega * log(InLog);
      std::complex<double> alphadot_t_complex = omega * sinh(omega * t) * alpha + cosh(omega*t) * alphadot + omega * sinh(omega * t) * beta/gamma;
      std::complex<double> alpha_t_complex = cosh(omega * t) * alpha + sinh(omega*t)/omega * alphadot + (cosh(omega * t)- 1.0) * beta/gamma;
      alpha_t_real = real(alpha_t_complex);
      double alphadot_t = real(alphadot_t_complex);
      alphadot_t = sqrt(alphadot_t * alphadot_t);
      alphaddot = beta + (alpha_t_real - alpha) * gamma;
      ResPair = std::make_pair(alphaupp, alphadot_t);
      return ResPair;
    }
    else
    {
      double alpha_t = alpha - 0.5 * alphadot * alphadot/beta;
      if(alpha_t<=alphaupp)
      {
        alphaddot = beta + (alpha_t - alpha) * gamma;
        ResPair = std::make_pair(alpha_t, 0.0);
        return ResPair;
      }
      double t = (-alphadot + sqrt(alphadot * alphadot - 2.0 * beta * (alpha - alphaupp)))/beta;
      alpha_t = alphaupp;
      double alphadot_t = alphadot + t * beta;
      alphaddot = beta + (alpha_t - alpha) * gamma;
      ResPair = std::make_pair(alpha_t, alphadot_t);
      return ResPair;
    }
  }
  return ResPair;
}

static double LinearInter(const Vector2& LeftPt, const Vector2& RightPt, const double& x_new)
{
  // This function is used to calculate the value for y_new based on the LeftPt and Rightpt.
  double x_offset = x_new - LeftPt.x;
  double k = (RightPt.y - LeftPt.y)/(RightPt.x - LeftPt.x);
  double y_new = LeftPt.y + k * x_offset;
  return y_new;
}

static void AccLinearCoeff(const double & Alpha, const std::vector<Vector2> & Path, double & Beta, double & Gamma, double & AlphaUpp, const double & AlphaFeasMax)
{
  // This function is used to find out the linear coefficients for acceleration function.
  for (int i = 0; i < Path.size()-1; i++)
  {
    if((Alpha>=Path[i].x)&&(Alpha<=Path[i+1].x))
    {
      // This is used to bound the range of alpha.
      Vector2 LeftPt = Path[i];
      Vector2 RightPt = Path[i+1];

      Gamma = (RightPt.y - LeftPt.y)/(RightPt.x - LeftPt.x);
      Beta = LeftPt.y - Gamma * LeftPt.x;

      if(RightPt.y<0)
      {
        AlphaUpp = RightPt.x;
      }
      else
      {
        AlphaUpp = LeftPt.x - LeftPt.y/Gamma;
      }
    }
  }
  if(AlphaUpp>=AlphaFeasMax)
  {
    AlphaUpp = AlphaFeasMax;
  }
}

static int LinearCoeff(const double & alpha, const FacetInfo & FacetObj, const int & Flag, double & beta_i, double & gamma_i, double & alphaupp_i)
{
  // This function is used to calculate the linear coefficient: alphaddot = beta_i + gamma_i * alpha.
  std::vector<int> EdgeIndices;
  std::vector<Vector2> LeftPts, RightPts;
  std::vector<double> InterAlphaddot;
  for (int i = 0; i < FacetObj.FacetEdges.size(); i++)
  {
    double alphaleft = FacetObj.FacetEdges[i].first.x - alpha;
    double alpharight = FacetObj.FacetEdges[i].second.x - alpha;
    if(((alphaleft<=0)&&(alpharight>=0))||((alphaleft >=0)&&(alpharight<=0)))
    {
      // Then we switch the order of first, second to make sure that the second point is on the right.
      Vector2 LeftPt, RightPt;
      if(FacetObj.FacetEdges[i].second.x>FacetObj.FacetEdges[i].first.x)
      {
        LeftPt.x = FacetObj.FacetEdges[i].first.x;
        LeftPt.y = FacetObj.FacetEdges[i].first.y;

        RightPt.x = FacetObj.FacetEdges[i].second.x;
        RightPt.y = FacetObj.FacetEdges[i].second.y;
      }
      else
      {
        LeftPt.x = FacetObj.FacetEdges[i].second.x;
        LeftPt.y = FacetObj.FacetEdges[i].second.y;

        RightPt.x = FacetObj.FacetEdges[i].first.x;
        RightPt.y = FacetObj.FacetEdges[i].first.y;
      }
      if((RightPt.x - alpha)>0)
      {
        // This means that we only choose the point on the right side
        EdgeIndices.push_back(i);
        LeftPts.push_back(LeftPt);
        RightPts.push_back(RightPt);
        double alphaddot = LinearInter(LeftPt, RightPt, alpha);
        InterAlphaddot.push_back(alphaddot);
      }
    }
  }
  if(InterAlphaddot.size() ==0)
  {
    // This means that the current region does not contain the origin to search for the feasible solution.
    return 2;
  }

  // In this case, we should sort alpha values according to the linear splines.
  int AlphaIndex;
  switch (Flag)
  {
    case 0:
    {
      AlphaIndex = std::max_element(InterAlphaddot.begin(), InterAlphaddot.end()) - InterAlphaddot.begin();
    }
    break;
    case 1:
    {
      AlphaIndex = std::min_element(InterAlphaddot.begin(), InterAlphaddot.end()) - InterAlphaddot.begin();
    }
    break;
    default:
    break;
  }
  Vector2 LeftPt = LeftPts[AlphaIndex];
  Vector2 RightPt = RightPts[AlphaIndex];

  gamma_i = (RightPt.y - LeftPt.y)/(RightPt.x - LeftPt.x);
  beta_i = InterAlphaddot[AlphaIndex];
  if(RightPt.y<0)
  {
    alphaupp_i = RightPt.x;
  }
  else
  {
    alphaupp_i = LeftPt.x - LeftPt.y/gamma_i;
  }
  return 1;
}

static Vector2 AppendPoint(const Vector2 & Point, const FacetInfo & FacetObj, bool & PointAdditionFlag)
{
  Vector2 NextPoint;
  for (int i = 0; i < FacetObj.FacetEdges.size(); i++)
  {
    double FirstCom_x = FacetObj.FacetEdges[i].first.x - Point.x;
    double FirstCom_y = FacetObj.FacetEdges[i].first.y - Point.y;
    double FirstComVal = FirstCom_x * FirstCom_x + FirstCom_y * FirstCom_y;

    double SecondCom_x = FacetObj.FacetEdges[i].second.x - Point.x;
    double SecondCom_y = FacetObj.FacetEdges[i].second.y - Point.y;
    double SecondComVal = SecondCom_x * SecondCom_x + SecondCom_y * SecondCom_y;

    if(FirstComVal<eps)
    {
      // This means that First Point is the Point
      if(SecondCom_x>0)
      {
        NextPoint.x = FacetObj.FacetEdges[i].second.x;
        NextPoint.y = FacetObj.FacetEdges[i].second.y;

        PointAdditionFlag = true;
        return NextPoint;
      }
    }

    if(SecondComVal<eps)
    {
      if(FirstCom_x>0)
      {
        NextPoint.x = FacetObj.FacetEdges[i].first.x;
        NextPoint.y = FacetObj.FacetEdges[i].first.y;

        PointAdditionFlag = true;
        return NextPoint;
      }
    }
  }
  PointAdditionFlag = false;
  return NextPoint;
}

static void FindPathToTheEnd(std::vector<Vector2> & Path, const FacetInfo & FacetObj)
{
  bool PointAdditionFlag = true;
  while(PointAdditionFlag)
  {
    // try to find the path connecting to the last point from path
    Vector2 NextPoint;
    NextPoint = AppendPoint(Path[Path.size()-1], FacetObj, PointAdditionFlag);
    switch (PointAdditionFlag)
    {
      case true:
      {
        Path.push_back(NextPoint);
      }
      break;
      default:
      {
      }
      break;
    }
  }
}

static bool PathToTheEnd(const FacetInfo & FacetObj, std::vector<Vector2> & UpperPath, std::vector<Vector2> & LowerPath)
{
  // This function is used to find the path from 0 to the end.
  bool PathFlag = false;
  std::vector<Vector2> StartPointsLeft, StartPointsRight;
  std::vector<int> InterSegIndices;
  for (int i = 0; i < FacetObj.FacetEdges.size(); i++)
  {
    // The first element is the alpha while the second element is the alpha ddot.
    double Sign = FacetObj.FacetEdges[i].first.x * FacetObj.FacetEdges[i].second.x;
    if (Sign<0)
    {
      double delta_y = FacetObj.FacetEdges[i].first.y - FacetObj.FacetEdges[i].second.y;
      double delta_x = FacetObj.FacetEdges[i].first.x - FacetObj.FacetEdges[i].second.x;
      double k = delta_y/delta_x;

      double StartPointLeft_y = FacetObj.FacetEdges[i].first.y - k * FacetObj.FacetEdges[i].first.x;
      Vector2 StartPointLeft(0.0, StartPointLeft_y);
      StartPointsLeft.push_back(StartPointLeft);
      if(FacetObj.FacetEdges[i].first.x>FacetObj.FacetEdges[i].second.x)
      {
        Vector2 StartPointRight(FacetObj.FacetEdges[i].first.x, FacetObj.FacetEdges[i].first.y);
        StartPointsRight.push_back(StartPointRight);
      }
      else
      {
        Vector2 StartPointRight(FacetObj.FacetEdges[i].second.x, FacetObj.FacetEdges[i].second.y);
        StartPointsRight.push_back(StartPointRight);
      }
      InterSegIndices.push_back(i);
    }
    else
    {
      if(Sign == 0)
      {
        double FirstAtZero = FacetObj.FacetEdges[i].first.x * FacetObj.FacetEdges[i].first.x;
        if(FirstAtZero<eps)
        {
          if(FacetObj.FacetEdges[i].first.x<FacetObj.FacetEdges[i].second.x)
          {
            Vector2 StartPointLeft(FacetObj.FacetEdges[i].first.x, FacetObj.FacetEdges[i].first.y);
            Vector2 StartPointRight(FacetObj.FacetEdges[i].second.x, FacetObj.FacetEdges[i].second.y);
            StartPointsLeft.push_back(StartPointLeft);
            StartPointsRight.push_back(StartPointRight);
            InterSegIndices.push_back(i);
          }
        }
        else
        {
          if(FacetObj.FacetEdges[i].first.x>FacetObj.FacetEdges[i].second.x)
          {
            Vector2 StartPointLeft(FacetObj.FacetEdges[i].second.x, FacetObj.FacetEdges[i].second.y);
            Vector2 StartPointRight(FacetObj.FacetEdges[i].first.x, FacetObj.FacetEdges[i].first.y);
            StartPointsLeft.push_back(StartPointLeft);
            StartPointsRight.push_back(StartPointRight);
            InterSegIndices.push_back(i);
          }
        }
      }
    }
  }

  switch (StartPointsLeft.size())
  {
    case 0:
    {
      return false;
    }
    break;
    case 3:
    {
      std::cerr<<"Intersections cannot have more than 2 segments!"<<endl;
      return false;
    }
    break;
    default:
    {
    }
    break;
  }

  // Now the job is to figure out which one is at top and which one is at bottom.
  if(StartPointsLeft[0].y > StartPointsLeft[1].y)
  {
    UpperPath.push_back(StartPointsLeft[0]);
    UpperPath.push_back(StartPointsRight[0]);

    LowerPath.push_back(StartPointsLeft[1]);
    LowerPath.push_back(StartPointsRight[1]);
  }
  else
  {
    UpperPath.push_back(StartPointsLeft[1]);
    UpperPath.push_back(StartPointsRight[1]);

    LowerPath.push_back(StartPointsLeft[0]);
    LowerPath.push_back(StartPointsRight[0]);
  }

  // Alright, let's find the path from this point to the most right end.
   FindPathToTheEnd(UpperPath, FacetObj);
   FindPathToTheEnd(LowerPath, FacetObj);

  return true;
}

static bool TooFastCaseValidation(const double & AlphadotInit, const std::vector<Vector2> & UppPath, const double & AlphaFeasMin, const double & AlphaFeasMax)
{
  // This main purpose of this function is to validate whether robot can reach feasible alpha region.
  // This computation will be terminated if any of these two conditions has been triggered.
  // 1. alpha reaches feasible region.
  // 2. alphadot has been declined to zero.
  double Alpha = 0.0;
  double Alphadot = AlphadotInit;
  double Beta = 0.0;
  double Gamma = 0.0;
  double AlphaUpp = 0.0;
  double Alphaddot = 0.0;

  int IterLimit = 20;
  for (int i = 0; i < IterLimit; i++)
  {
    AccLinearCoeff(Alpha, UppPath, Beta, Gamma, AlphaUpp, AlphaFeasMax);
    std::pair<double, double> AlphaNAlphadot = IntegratorLDS(Alpha, Alphadot, Alphaddot, Beta, Gamma, AlphaUpp);
    Alpha = AlphaNAlphadot.first;
    Alphadot = AlphaNAlphadot.second;

    // Condition 1
    if(Alpha>=AlphaFeasMin)
    {
      return true;
    }

    // Condition 2
    if((Alphadot==0.0)&&(Alpha<AlphaFeasMin))
    {
      return false;
    }
  }
  return false;
}

static bool ZSCEvaluation(const double & AlphadotInit, const std::vector<Vector2> & LowPath, const std::vector<Vector2> & UppPath, const double & AlphaFeasMin, const double & AlphaFeasMax, const double & AlphaMax)
{
  // This function is used to conduct the evaluation of ZSC.
  int MaxIter = 100;
  int CurIter = 0;
  double Alpha = 0.0;
  double Alphadot = AlphadotInit;
  double Alphaddot = 0.0;

  double Beta = 0.0;
  double Gamma = 0.0;
  double AlphaUpp = 0.0;          // This is the Upper Bound for a certain integration.

  bool StabilizedFlag = false;
  while (CurIter<MaxIter)
  {
    if((Alphadot>-eps)&&(Alphadot<eps))
    {
      // In this case, Alphadot vanishes!
      if((Alpha=AlphaFeasMin)&&(Alpha<=AlphaFeasMax))
      {
        // C3.1
        // Tested!
        StabilizedFlag = true;
        // std::printf("C3.1\n");
        return StabilizedFlag;
      }
      if(Alpha<AlphaFeasMin)
      {
        // C3.2
        // This is an annoying case.
        StabilizedFlag = TooFastCaseValidation(AlphadotInit, UppPath, AlphaFeasMin, AlphaFeasMax);
        // std::printf("C3.2\n");
        return StabilizedFlag;
      }
      if(Alpha>AlphaFeasMax)
      {
        // C3.3
        StabilizedFlag = false;
        // std::printf("C3.3\n");
        return StabilizedFlag;
      }
    }
    else
    {
      // In this case, robot's centroidal velocity is not zero.
      if(Alpha>=AlphaFeasMax)
      {
        // C1: Velocity is too large to be compensated!
        // Tested!
        StabilizedFlag = false;
        // std::printf("C1\n");
        return StabilizedFlag;
      }
      if(Alpha>=AlphaMax)
      {
        // C2: Position arrives at the position where constraints will be violated!
        // Tested!
        StabilizedFlag = false;
        // std::printf("C2\n");
        return StabilizedFlag;
      }
    }
    AccLinearCoeff(Alpha, LowPath, Beta, Gamma, AlphaUpp, AlphaFeasMax);
    std::pair<double, double> AlphaNAlphadot = IntegratorLDS(Alpha, Alphadot, Alphaddot, Beta, Gamma, AlphaUpp);
    Alpha = AlphaNAlphadot.first;
    Alphadot = AlphaNAlphadot.second;

    // std::printf("Alpha: %f and Alphadot: %f\n", Alpha, Alphadot);
    CurIter = CurIter + 1;
  }
  return StabilizedFlag;
}

static bool ZSCInner(const std::vector<Vector3> & ActContactPositions, const std::vector<Vector3> & ConeUnit, const int & EdgeNo, const Vector3& COMPos, const Vector3& COMVel)
{
  int HFailureFlag;
  Matrix H = PolyhedralProjection(ActContactPositions, ConeUnit, EdgeNo, HFailureFlag);
  switch (HFailureFlag)
  {
    case 1:
    {
      // This means that Zero-step capturability method does not work
      return false;
    }
    break;
    default:
    break;
  }

  std::vector<Vector3> ConeVertices = CentroidalCone(H, COMPos, COMVel);
  if(ConeVertices.size()<3)
  {
    // In this case, there is no need to conduct further computation.
    return false;
  }

  int CollinearFlag = CollinearTest(ActContactPositions);
  switch (CollinearFlag)
  {
    case 1:
    {
      return false;
    }
    break;
    default:
    break;
  }

  int FacetFlag = 0;
  FacetInfo FacetObj = FlatContactHullGeneration(ConeVertices, FacetFlag);
  switch (FacetFlag)
  {
    case 0:
    {
      return false;
    }
    break;
    default:
    break;
  }

  // Now let's compute the maximum deaccelerated velocity.
  bool StabilizedFlag = true;
  /*
    The first job is to compute the intersection where the acceleration vanishes.
  */
  std::vector<double> AlphaFeasible;
  std::vector<double> AlphaTotal(FacetObj.FacetEdges.size());
  for (int i = 0; i < FacetObj.FacetEdges.size(); i++)
  {
    // The first element is the alpha while the second element is the alpha ddot.
    double Sign = FacetObj.FacetEdges[i].first.y * FacetObj.FacetEdges[i].second.y;
    if (Sign<0)
    {
      double delta_y = FacetObj.FacetEdges[i].first.y - FacetObj.FacetEdges[i].second.y;
      double delta_x = FacetObj.FacetEdges[i].first.x - FacetObj.FacetEdges[i].second.x;
      double k = delta_y/delta_x;
      double Alpha = FacetObj.FacetEdges[i].first.x - FacetObj.FacetEdges[i].first.y/k;
      AlphaFeasible.push_back(Alpha);
    }
    else
    {
      if(Sign == 0)
      {
        double FirstAtZero = FacetObj.FacetEdges[i].first.y * FacetObj.FacetEdges[i].first.y;
        if(FirstAtZero<eps)
        {
          AlphaFeasible.push_back(FacetObj.FacetEdges[i].first.x);
        }
        else
        {
          AlphaFeasible.push_back(FacetObj.FacetEdges[i].second.x);
        }
      }
    }
    AlphaTotal[i] = FacetObj.FacetEdges[i].first.x;
  }
  switch (AlphaFeasible.size())
  {
    case 0:
    {
      return false;
    }
    break;
    default:
    break;
  }

  double AlphaFeasMin = *std::min_element(AlphaFeasible.begin(), AlphaFeasible.end());
  double AlphaFeasMax = *std::max_element(AlphaFeasible.begin(), AlphaFeasible.end());

  double AlphaTotalMin = *std::min_element(AlphaTotal.begin(), AlphaTotal.end());
  double AlphaTotalMax = *std::max_element(AlphaTotal.begin(), AlphaTotal.end());

  std::vector<Vector2> UpperPath, LowerPath;
  PathToTheEnd(FacetObj, UpperPath, LowerPath);

  switch (UpperPath.size())
  {
    case 0:
    {
      return false;
    }
    break;
    case 1:
    {
      return false;
    }
    break;
    default:
    {
    }
    break;
  }

  switch (LowerPath.size())
  {
    case 0:
    {
      return false;
    }
    break;
    case 1:
    {
      return false;
    }
    break;
    default:
    {
    }
    break;
  }

  double AlphadotInit = sqrt(COMVel.x * COMVel.x + COMVel.y * COMVel.y + COMVel.z * COMVel.z);

  StabilizedFlag = ZSCEvaluation(AlphadotInit, LowerPath, UpperPath, AlphaFeasMin, AlphaFeasMax, AlphaTotalMax);

  return StabilizedFlag;
}

double ZeroStepCapturabilityGenerator(const std::vector<Vector3> & ActContactPositions, const std::vector<Vector3> & ConeUnit, const int & EdgeNo, const Vector3& COMPos, const Vector3& COMVel)
{
  // This function is used to generate the solution for zero capturability failure metric.
  bool StabilizedFlag;
  try
  {
    StabilizedFlag = ZSCInner(ActContactPositions, ConeUnit, EdgeNo, COMPos, COMVel);
  }
  catch(...)
  {
    StabilizedFlag = false;
  }
  double ZSCObj = 1.0;
  switch (StabilizedFlag)
  {
    case true:
    {
      // This means that the robot can be stabilized using this method.
      ZSCObj = 0.0;
    }
    break;
    default:
    {
      // This means that the robot cannot be stabilized using this metric.
      ZSCObj = 1.0;
    }
    break;
  }
  return ZSCObj;
}


/*=========================================================================
 *
 *  Copyright David Doria 2012 daviddoria@gmail.com
 *
 *  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.txt
 *
 *  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.
 *
 *=========================================================================*/

#ifndef LinearSearchKNNPropertyNoReuse_HPP
#define LinearSearchKNNPropertyNoReuse_HPP

// STL
#include <algorithm> // for lower_bound()
#include <limits> // for infinity()
#include <set>

// Boost
#include <boost/utility.hpp> // for enable_if()
#include <boost/type_traits.hpp> // for is_same()

// Custom
#include "Utilities/Utilities.hpp"

/**
  * This function template is similar to std::min_element but can be used when the comparison
  * involves computing a derived quantity (a.k.a. distance). This algorithm will search for the
  * the elements in the range [first,last) with the "smallest" distances (of course, both the
  * distance metric and comparison can be overriden to perform something other than the canonical
  * Euclidean distance and less-than comparison, which would yield the element with minimum distance).
  * This function will fill the output container with a number of nearest-neighbors.
  * \tparam DistanceValueType The value-type for the distance measures.
  * \tparam DistanceFunctionType The functor type to compute the distance measure.
  */
template <typename PropertyMapType,
          typename PatchDistanceFunctionType>
class LinearSearchKNNPropertyNoReuse
{
  typedef float DistanceValueType;

  /** The map of vertex descriptors. */
  PropertyMapType PropertyMap;

  /** How many top patches to return. */
  unsigned int K;

  /** The function to use to compare patches. */
  PatchDistanceFunctionType PatchDistanceFunction;

  /** Store a pointer to the InpaintingVisitor's UsedNodesSet */
  typedef std::set<itk::Index<2>, itk::Index<2>::LexicographicCompare> UsedNodesSetType;
  UsedNodesSetType* UsedNodesSet;

public:
  LinearSearchKNNPropertyNoReuse(PropertyMapType propertyMap, const unsigned int k = 1000,
                                 PatchDistanceFunctionType patchDistanceFunction = PatchDistanceFunctionType(),
                                 UsedNodesSetType* usedNodesSet = nullptr) :
    PropertyMap(propertyMap), K(k), PatchDistanceFunction(patchDistanceFunction), UsedNodesSet(usedNodesSet)
  {
  }

  /** Set the number of nearest neighbors to return. */
  void SetK(const unsigned int k)
  {
    this->K = k;
  }

  /** Get the number of nearest neighbors to return. */
  unsigned int GetK() const
  {
    return this->K;
  }

  //   template <typename ForwardIteratorType, typename OutputContainerType>
  //   void operator()(ForwardIteratorType first, ForwardIteratorType last,
  //                   typename ForwardIteratorType::value_type queryNode, OutputContainerType& output)
  /**
    * \tparam ForwardIterator The forward-iterator type.
    * \tparam OutputContainer The container type which can contain the list of nearest-neighbors (STL like container, with iterators, insert, size, and pop_back).
    * \param first Start of the range in which to search.
    * \param last One element past the last element in the range in which to search (usually container.end() ).
    * \param output The container that will have the sorted list of elements with the smallest distance.
    * \param compare A callable object that returns true if the first element is the preferred one (less-than) of the two.
    * \param max_neighbors The maximum number of elements of smallest distance to output in the sorted list.
    */
  template <typename ForwardIteratorType, typename OutputIteratorType>
  inline OutputIteratorType
  operator()(ForwardIteratorType first, ForwardIteratorType last,
             typename ForwardIteratorType::value_type queryNode,
             OutputIteratorType outputFirst)
  {
    // Nothing to do if the input range is empty
    if(first == last)
    {
      return outputFirst;
    }

    // Use a priority queue to keep the items sorted
    typedef std::pair<DistanceValueType, ForwardIteratorType> PairType;
    typedef compare_pair_first<DistanceValueType, ForwardIteratorType, std::greater<DistanceValueType> > CompareType;
    typedef std::priority_queue< PairType, std::vector<PairType>,
        CompareType> PriorityQueueType;

    PriorityQueueType outputQueue;

    typename PropertyMapType::value_type queryPatch = get(this->PropertyMap, queryNode);

    typedef typename ForwardIteratorType::value_type NodeType;

    // The queue stores the items in descending score order.
    #pragma omp parallel for
//    for(ForwardIteratorType current = first; current != last; ++current) // OpenMP 3 doesn't allow != in the loop ending condition
    for(ForwardIteratorType currentIterator = first; currentIterator < last; ++currentIterator)
    {
      NodeType currentNode = *currentIterator;

      itk::Index<2> currentIndex = Helpers::ConvertFrom<itk::Index<2>, NodeType>(currentNode);
      UsedNodesSetType::iterator usedNodesSetIterator = this->UsedNodesSet->find(currentIndex);

      if(usedNodesSetIterator == this->UsedNodesSet->end()) // not already used
      {
        DistanceValueType d = this->PatchDistanceFunction(get(this->PropertyMap, currentNode), queryPatch); // (source, target) (the query node is the target node)

        #pragma omp critical // There are weird crashes without this guard
        outputQueue.push(PairType(d, currentIterator));
      }
      else
      {
        std::cout << "Prevented use." << std::endl;
      }
    }

//    std::cout << "There are " << outputQueue.size() << " items in the queue." << std::endl;

    // Keep only the best K matches
    Helpers::KeepTopN(outputQueue, this->K);

//    std::cout << "There are " << outputQueue.size() << " items in the queue." << std::endl;

    if(outputQueue.size() < this->K)
    {
      std::stringstream ss;
      ss << "Requested " << this->K << " items but only found " << outputQueue.size();
      throw std::runtime_error(ss.str());
    }

    std::cout << "Best patch score is: " << outputQueue.top().first << std::endl;

    // Copy the best matches from the queue into the output
    OutputIteratorType currentOutputIterator = outputFirst;
    while( !outputQueue.empty() )
    {
      *currentOutputIterator = *(outputQueue.top().second);
      outputQueue.pop();
      ++currentOutputIterator;
    }

    return currentOutputIterator;
  } // end operator()

  template <typename T1, typename T2, typename Compare>
  struct compare_pair_first : std::binary_function< std::pair<T1, T2>, std::pair<T1, T2>, bool>
  {
    Compare comp;
    compare_pair_first(const Compare& aComp = Compare()) : comp(aComp) { }
    bool operator()(const std::pair<T1, T2>& x, const std::pair<T1, T2>& y) const
    {
      return comp(x.first, y.first);
    }
  };

  /** This is the case where the output iterators contain nodes.
    * 'result' does not have to be passed by reference because it is an iterator.
    * The return type if this function is enabled is OutputIteratorType.
    */
  template <typename PairPriorityQueueType,
  typename OutputIteratorType>
  inline
  typename boost::enable_if<
  boost::is_same<
  typename std::iterator_traits<typename PairPriorityQueueType::value_type::second_type >::value_type,
  typename std::iterator_traits< OutputIteratorType >::value_type
  >,
  OutputIteratorType >::type copy_neighbors_from_queue(PairPriorityQueueType& Q, OutputIteratorType result)
  {
    OutputIteratorType first = result;
    while( !Q.empty() )
    {
      *result = *(Q.top().second);
      Q.pop();
      ++result;
    };
    std::reverse(first, result);
    return result;
  }

};

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <complex>
#include <vector>

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

const unsigned sz= 5;

inline float f(float x) { return x; }
inline double f(double x) { return x; }

inline std::complex<double> f(std::complex<double> x)
{
    return std::complex<double>(real(x), real(x)+1.0);
}

template <typename Vector>
void test(Vector& v, const char* name)
{
    typedef typename mtl::Collection<Vector>::value_type T;
    using std::abs; using std::cout;
    std::cout << "\n" << name << "\n";

    for (unsigned i= 0; i < size(v); ++i)
	v[i]= f(T(i));

    Vector w(sub_vector(v, 2, 4));
    w[1]= f(T(8));
    cout << "w == " << w << "\n";

    MTL_THROW_IF(w[0] != f(T(2)), mtl::runtime_error("Wrong value in w"));
    MTL_THROW_IF(size(w) != 2, mtl::runtime_error("Wrong size of w"));

    MTL_THROW_IF(v[3] != f(T(8)), mtl::runtime_error("Cannot change v via w (correctly)"));

    Vector u(sub_vector(v, 2, 7));
    cout << "u == " << u << "\n";
    cout << "v == " << v << "\n";

    MTL_THROW_IF(u[0] != f(T(2)), mtl::runtime_error("Wrong value in u"));
    MTL_THROW_IF(size(u) != sz-2, mtl::runtime_error("Wrong size of u"));

    MTL_THROW_IF(size(sub_vector(v, 2, 2)) != 0, mtl::runtime_error("Problem returning empty vector (2, 2)"));
    MTL_THROW_IF(size(sub_vector(v, 4, 2)) != 0, mtl::runtime_error("Problem returning empty vector (4, 2)"));
    MTL_THROW_IF(size(sub_vector(v, 8, 9)) != 0, mtl::runtime_error("Problem returning empty vector (8, 9)"));

#if 0
    const Vector vc(v);
    cout << "vc == " << vc << "\n";

    Vector wc(sub_vector(vc, 2, 4));
    const Vector cwc(sub_vector(vc, 2, 4));
    cout << "wc == " << wc << "\n";

    MTL_THROW_IF(wc[0] != f(T(2)), mtl::runtime_error("Wrong value in wc"));
    MTL_THROW_IF(size(wc) != 2, mtl::runtime_error("Wrong size of wc"));

    wc[1]= f(T(5));
    cout << "vc == " << vc << "\n";
#endif
}

int main(int, char**)
{
    mtl::dense_vector<float>                                                 cf(sz, 1.0);
    mtl::dense_vector<double>                                                cd(sz, 1.0);
    mtl::dense_vector<std::complex<double> >                                 cc(sz, 1.0);
    mtl::dense_vector<float, mtl::vec::parameters<mtl::row_major> >          rf(sz, 1.0);

    test(cf, "dense_vector<float>");
    test(cd, "dense_vector<double>");
    test(cc, "dense_vector<std::complex<double> >");
    test(rf, "dense_vector<float, parameters<row_major> >");

    return 0;
}


#ifndef BOOZ_VECTOR_MATRIX_H
#define BOOZ_VECTOR_MATRIX_H

#include <Eigen/Core>

namespace booz_xform {

  typedef double boozfloat;

  typedef Eigen::ArrayXd Vector;
  typedef Eigen::ArrayXi IntVector;
  typedef Eigen::ArrayXXd Matrix;

} // namespace booz_xform

#endif


#pragma once

// MTL4 includes
#include <boost/numeric/mtl/mtl.hpp> // TODO: why not using mtl_fwd.hpp??

// AMDiS includes
#include "OpenMP.hpp"
#include "Config.hpp"

namespace AMDiS
{
  class AdaptInfo;
  class AdaptStationary;
  class Assembler;
  class BasisFunction;
  struct BlockMTLMatrix;
  class BoundaryManager;
  //   class CGSolver;
  class CoarseningManager;
  class CouplingIterationInterface;
  //   class DiagonalPreconditioner;
  class DOFAdmin;
  class DOFContainer;
  class DOFIndexedBase;
  class DOFMatrix;
  class DOFVectorDOF;
  class Element;
  class ElementDofIterator;
  class ElInfo;
  class ElMatrixInfo;
  class Estimator;
  class FastQuadrature;
  class FillInfo;
  class FileWriterInterface;
  class FiniteElemSpace;
  class Flag;
  //   class IdentityPreconditioner;
  //  class Parallel::InteriorBoundary;
  class LeafDataPeriodic;
  class LevelAdmin;
  class Line;
  class MacroElement;
  class MacroInfo;
  class Marker;
  class Mesh;
  class MeshAccessor;
  class MeshStructure;
  class LinearSolverInterface;
  class Operator;
  class OperatorTerm;
  class Parametric;
  class PeriodicBC;
  class ProblemInstat;
  class ProblemInstatScal;
  class ProblemInstatVec;
  class ProblemIterationInterface;
  class ProblemStatBase;
  class ProblemStatSeq;
  class ProblemTimeInterface;
  class Projection;
  class PreconditionerScal;
  class Quadrature;
  class Q00PsiPhi;
  class Q0Psi;
  class Q10PsiPhi;
  class Q01PsiPhi;
  class Q1Psi;
  class RCNeighbourList;
  class RefinementManager;
  class RosenbrockAdaptInstationary;
  class RosenbrockStationary;
  class RobinBC;
  class SubElInfo;
  class SurfaceOperator;
  class SMIAdapter;
  class SystemVector;
  class Tetrahedron;
  class TraverseStack;
  class Triangle;
  struct VertexInfo;
  class VertexVector;

  namespace io
  {
    class ElementFileWriter;
    class GNUPlotWriter;
    class MacroReader;
    struct MacroWriter;
    class PngWriter;
    class PovrayWriter;
    class Spreadsheet;
  }

#ifdef HAVE_PARALLEL_DOMAIN_AMDIS
  namespace Parallel
  {
    class ElementObjectDatabase;
    class FeSpaceDofMap;
    class MatrixNnzStructure;
    class MeshLevelData;
    class PetscSolver;
    class PetscSolverFeti;
    class PetscSolverFetiDebug;
    class PetscSolverSchur;
    class PetscSolverGlobalMatrix;
    class PetscSolverGlobalBlockMatrix;
    class PetscSolverCahnHilliard;
    class PetscSolverNavierStokes;
    class ParallelDofMapping;
    class ParallelProblemStat;
    class ParallelSolver;
  }
#endif

  struct BoundaryObject;
  struct AtomicBoundary;

  template<typename T>                                 class DOFIndexed;
  template<typename T>                                 class DOFVectorBase;
  template<typename T>                                 class DOFVector;
  template<typename T>                                 class DirichletBC;
  //   template<typename ITLSolver>                         class ITL_LinearSolverInterface;
  template<typename T, typename MatT, typename VecT>  class ITL_Preconditioner;
  template<typename T>                                 class MatrixOfFixVecs;
  template<typename T>                                 class MatVecMultiplier;
  template<typename T>                                 class SolverMatrix;
  template<typename T>                                 class VectorOfFixVecs;

  namespace detail
  {
    template <class P> class CouplingProblemStat;
  }

  // some typedefs to introduce matrix and vector-types
  // ---------------------------------------------------------------------------

  template <class T>
  using DenseVector = mtl::dense_vector<T>;

  template <class T>
  using DenseMatrix = mtl::dense2D<T>;

  template <class T>
  using SparseMatrix = mtl::compressed2D<T>;

  // TODO: use templated ElementMatrix;
  using ElementMatrix = DenseMatrix<double>;

} // end namespace AMDiS


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/bitwise/include/functions/shr.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/three.hpp>

NT2_TEST_CASE_TPL ( shr_uit,  BOOST_SIMD_UNSIGNED_TYPES)
{

  using boost::simd::shr;
  using boost::simd::tag::shr_;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef typename boost::dispatch::meta::call<shr_(T,iT)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_EQUAL(shr(T(2),1), boost::simd::One<T>());
  NT2_TEST_EQUAL(shr(boost::simd::Valmax<T>(),(sizeof(T)*8-1)), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(shr(boost::simd::Valmax<T>(),(sizeof(T)*8-2)), boost::simd::Three<r_t>());
  NT2_TEST_EQUAL(shr(boost::simd::One<T>(),1), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(shr(boost::simd::Zero<T>(),1), boost::simd::Zero<r_t>());
} // end of test for unsigned_int_

NT2_TEST_CASE_TPL ( shr_si,  BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{

  using boost::simd::shr;
  using boost::simd::tag::shr_;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef typename boost::dispatch::meta::call<shr_(T,iT)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_EQUAL(shr(T(2),1), boost::simd::One<T>());
  NT2_TEST_EQUAL(shr(boost::simd::Mone<T>(),(sizeof(T)*8-1)), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(shr(boost::simd::Mone<T>(),(sizeof(T)*8-2)), boost::simd::Three<r_t>());
  NT2_TEST_EQUAL(shr(boost::simd::One<T>(),1), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(shr(boost::simd::Zero<T>(),1), boost::simd::Zero<r_t>());
} // end of test for unsigned_int_