Fixed the changing WL matrix bug in witness complex. Added is_witness_complex test

git-svn-id: svn+ssh://scm.gforge.inria.fr/svnroot/gudhi/branches/witness@615 636b058d-ea47-450e-bf9e-a15bfbe3eedb

Former-commit-id: 60917e75766b53d08240b510c0508e7d781b56d6

1 files changed, 886 insertions, 0 deletions

diff --git a/src/Witness_complex/include/gudhi/Relaxed_witness_complex.h b/src/Witness_complex/include/gudhi/Relaxed_witness_complex.h
new file mode 100644
index 00000000..c869628f
--- /dev/null
+++ b/src/Witness_complex/include/gudhi/Relaxed_witness_complex.h
@@ -0,0 +1,886 @@
+/*    This file is part of the Gudhi Library. The Gudhi library
+ *    (Geometric Understanding in Higher Dimensions) is a generic C++
+ *    library for computational topology.
+ *
+ *    Author(s):       Siargey Kachanovich
+ *
+ *    Copyright (C) 2015  INRIA Sophia Antipolis-Méditerranée (France)
+ *
+ *    This program is free software: you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation, either version 3 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License
+ *    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef GUDHI_RELAXED_WITNESS_COMPLEX_H_
+#define GUDHI_RELAXED_WITNESS_COMPLEX_H_
+
+#include <boost/container/flat_map.hpp>
+#include <boost/iterator/transform_iterator.hpp>
+#include <algorithm>
+#include <utility>
+#include "gudhi/reader_utils.h"
+#include "gudhi/distance_functions.h"
+#include "gudhi/Simplex_tree.h"
+#include <vector>
+#include <list>
+#include <set>
+#include <queue>
+#include <limits>
+#include <math.h>
+#include <ctime>
+#include <iostream>
+
+// Needed for nearest neighbours
+#include <CGAL/Cartesian_d.h>
+#include <CGAL/Search_traits.h>
+#include <CGAL/Search_traits_adapter.h>
+#include <CGAL/property_map.h>
+#include <CGAL/Epick_d.h>
+#include <CGAL/Orthogonal_k_neighbor_search.h>
+
+#include <boost/tuple/tuple.hpp>
+#include <boost/iterator/zip_iterator.hpp>
+#include <boost/iterator/counting_iterator.hpp>
+#include <boost/range/iterator_range.hpp>
+
+// Needed for the adjacency graph in bad link search
+#include <boost/graph/graph_traits.hpp>
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/graph/connected_components.hpp>
+
+namespace Gudhi {
+
+
+  /** \addtogroup simplex_tree
+   *  Witness complex is a simplicial complex defined on two sets of points in \f$\mathbf{R}^D\f$:
+   *  \f$W\f$ set of witnesses and \f$L \subseteq W\f$ set of landmarks. The simplices are based on points in \f$L\f$
+   *  and a simplex belongs to the witness complex if and only if it is witnessed (there exists a point \f$w \in W\f$ such that
+   *  w is closer to the vertices of this simplex than others) and all of its faces are witnessed as well. 
+   */
+  template<typename FiltrationValue = double,
+           typename SimplexKey      = int,
+           typename VertexHandle    = int>
+  class Witness_complex: public Simplex_tree<> {
+
+  private:
+    
+    struct Active_witness {
+      int witness_id;
+      int landmark_id;
+      Simplex_handle simplex_handle;
+      
+      Active_witness(int witness_id_, int landmark_id_, Simplex_handle simplex_handle_)
+        : witness_id(witness_id_),
+          landmark_id(landmark_id_),
+          simplex_handle(simplex_handle_)
+      {}
+    };
+    
+      
+
+
+  public:
+  
+    
+    /** \brief Type for the vertex handle.
+     *
+     * Must be a signed integer type. It admits a total order <. */
+    typedef VertexHandle Vertex_handle;
+    
+    /* Type of node in the simplex tree. */ 
+    typedef Simplex_tree_node_explicit_storage<Simplex_tree> Node;
+    /* Type of dictionary Vertex_handle -> Node for traversing the simplex tree. */
+    typedef typename boost::container::flat_map<Vertex_handle, Node> Dictionary;
+    typedef typename Dictionary::iterator Simplex_handle;
+  
+    typedef std::vector< double > Point_t;
+    typedef std::vector< Point_t > Point_Vector;
+    
+    typedef std::vector< Vertex_handle > typeVectorVertex;
+    typedef std::pair< typeVectorVertex, Filtration_value> typeSimplex;
+    typedef std::pair< Simplex_tree<>::Simplex_handle, bool > typePairSimplexBool;
+    
+    typedef int Witness_id;
+    typedef int Landmark_id;
+    typedef std::list< Vertex_handle > ActiveWitnessList;
+    
+  private:
+    /** Number of landmarks
+     */
+    int nbL;
+    /** Desired density
+     */
+    double density;
+
+  public:
+
+    /** \brief Set number of landmarks to nbL_
+     */
+    void setNbL(int nbL_)
+    {
+      nbL = nbL_;
+    }
+
+    /** \brief Set density to density_
+     */
+    void setDensity(double density_)
+    {
+      density = density_;
+    }
+    
+    /**
+     * /brief Iterative construction of the relaxed witness complex basing on a matrix of k nearest neighbours of the form {witnesses}x{landmarks} and (1+epsilon)-limit table {witnesses}*{landmarks} consisting of iterators of k nearest neighbor matrix.
+     * The line lengths can differ, however both matrices have the same corresponding line lengths.
+     */
+    
+    template< typename KNearestNeighbours,  typename OPELimits >
+    void relaxed_witness_complex(KNearestNeighbours & knn, OPELimits & rl)
+    //void witness_complex(std::vector< std::vector< Vertex_handle > > & knn)
+    {
+      std::cout << "**Start the procedure witness_complex" << std::endl;
+      //Construction of the active witness list
+      int nbW = knn.size();
+      //int nbL = knn.at(0).size();
+      typeVectorVertex vv;
+      //typeSimplex simplex;
+      //typePairSimplexBool returnValue;
+      //int counter = 0;
+      /* The list of still useful witnesses
+       * it will diminuish in the course of iterations
+       */
+      ActiveWitnessList active_w;// = new ActiveWitnessList();
+      for (int i=0; i != nbL; ++i) {
+        // initial fill of 0-dimensional simplices
+        // by doing it we don't assume that landmarks are necessarily witnesses themselves anymore
+        //counter++;
+        vv = {i};
+        insert_simplex(vv, Filtration_value(0.0));
+        /* TODO Error if not inserted : normally no need here though*/
+      }
+      int k=1; /* current dimension in iterative construction */
+      //std::cout << "Successfully added landmarks" << std::endl;
+      // PRINT2
+      //print_sc(root()); std::cout << std::endl;
+      for (int i=0; i != nbW; ++i)
+        active_w.push_back(i);
+      /*
+      int u,v;     // two extremities of an edge
+      if (nbL > 1) // if the supposed dimension of the complex is >0
+        {
+          for (int i=0; i != nbW; ++i)
+            {
+              // initial fill of active witnesses list
+              u = knn[i][0];
+              v = knn[i][1];
+              vv = {u,v};
+              this->insert_simplex(vv,Filtration_value(0.0));
+              //print_sc(root()); std::cout << std::endl;
+              //std::cout << "Added edges" << std::endl;
+            }
+          //print_sc(root());
+          
+        }
+      */
+      std::cout << "k=0, active witnesses: " << active_w.size() << std::endl;
+      //std::cout << "Successfully added edges" << std::endl;
+      //count_good = {0,0};
+      //count_bad = {0,0};
+      while (!active_w.empty() && k < nbL )
+        {
+	  //count_good.push_back(0);
+	  //count_bad.push_back(0);
+          //std::cout << "Started the step k=" << k << std::endl;
+          typename ActiveWitnessList::iterator aw_it = active_w.begin();
+          while (aw_it != active_w.end())
+            {
+              std::vector<int> simplex;
+              bool ok = add_all_faces_of_dimension(k, knn[*aw_it].begin(), rl[*aw_it].begin(), simplex, knn[*aw_it].end(), knn[*aw_it].end());
+              if (!ok)
+                active_w.erase(aw_it++); //First increase the iterator and then erase the previous element
+              else
+                aw_it++;
+            }
+	  std::cout << "k=" << k << ", active witnesses: " << active_w.size() << std::endl;
+          k++;
+        }
+      //print_sc(root()); std::cout << std::endl;
+    }
+
+    /* \brief Adds recursively all the faces of a certain dimension dim witnessed by the same witness
+     * Iterator is needed to know until how far we can take landmarks to form simplexes
+     * simplex is the prefix of the simplexes to insert
+     * The output value indicates if the witness rests active or not
+     */
+    bool add_all_faces_of_dimension(int dim, std::vector<int>::iterator curr_l, typename std::vector< std::vector<int>::iterator >::iterator curr_until, std::vector<int>& simplex, std::vector<int>::iterator until, std::vector<int>::iterator end)
+    {
+      /*
+      std::ofstream ofs ("stree_result.txt", std::ofstream::out);
+      st_to_file(ofs);
+      ofs.close();
+      */
+      //print_sc(root());
+      bool will_be_active = false;
+      if (dim > 0)
+        for (std::vector<int>::iterator it = curr_l; it != until && it != end; ++it, ++curr_until)
+          {
+            simplex.push_back(*it);
+            if (find(simplex) != null_simplex())
+              will_be_active = will_be_active || add_all_faces_of_dimension(dim-1, it+1, curr_until+1, simplex, until, end);
+            simplex.pop_back();
+            if (until == end)
+              until = *curr_until;
+          }
+      else if (dim == 0)
+        for (std::vector<int>::iterator it = curr_l; it != until && it != end; ++it, ++curr_until)
+          {
+            simplex.push_back(*it);
+            if (all_faces_in(simplex))
+              {
+                will_be_active = true;
+                insert_simplex(simplex, 0.0);
+              }
+            simplex.pop_back();
+            if (until == end)
+              until = *curr_until;
+          }
+      return will_be_active;
+    }
+    
+    /** \brief Construction of witness complex from points given explicitly
+     *  nbL must be set to the right value of landmarks for strategies
+     * FURTHEST_POINT_STRATEGY and RANDOM_POINT_STRATEGY and
+     * density must be set to the right value for DENSITY_STRATEGY
+     */
+  // void witness_complex_from_points(Point_Vector point_vector)
+  // {
+  //   std::vector<std::vector< int > > WL;
+  //   landmark_choice_by_random_points(point_vector, point_vector.size(), WL);
+  //   witness_complex(WL);
+  // }
+  
+private:
+
+    /** \brief Print functions
+    */
+    void print_sc(Siblings * sibl)
+    {
+      if (sibl == NULL)
+        std::cout << "&";
+      else
+        print_children(sibl->members_);
+    }
+    
+    void print_children(Dictionary map)
+    {
+      std::cout << "(";
+      if (!map.empty())
+        {
+          std::cout << map.begin()->first;
+          if (has_children(map.begin()))
+            print_sc(map.begin()->second.children());
+          typename Dictionary::iterator it;
+          for (it = map.begin()+1; it != map.end(); ++it)
+            {
+              std::cout << "," << it->first;
+              if (has_children(it))
+                print_sc(it->second.children());
+            }
+        }
+      std::cout << ")";
+    }
+
+  public:
+    /** \brief Print functions
+     */
+
+    void st_to_file(std::ofstream& out_file)
+    {
+      sc_to_file(out_file, root());
+    }
+
+  private:
+    void sc_to_file(std::ofstream& out_file, Siblings * sibl)
+    {
+      assert(sibl);
+      children_to_file(out_file, sibl->members_);
+    }
+    
+    void children_to_file(std::ofstream& out_file, Dictionary& map)
+    {
+      out_file << "(" << std::flush;
+      if (!map.empty())
+        {
+          out_file << map.begin()->first << std::flush;
+          if (has_children(map.begin()))
+            sc_to_file(out_file, map.begin()->second.children());
+          typename Dictionary::iterator it;
+          for (it = map.begin()+1; it != map.end(); ++it)
+            {
+              out_file << "," << it->first << std::flush;
+              if (has_children(it))
+                sc_to_file(out_file, it->second.children());
+            }
+        }
+      out_file << ")" << std::flush;
+    }
+
+
+    /** \brief Check if the facets of the k-dimensional simplex witnessed 
+     *  by witness witness_id are already in the complex.
+     *  inserted_vertex is the handle of the (k+1)-th vertex witnessed by witness_id
+     */
+    bool all_faces_in(std::vector<int>& simplex)
+    {
+      //std::cout << "All face in with the landmark " << inserted_vertex << std::endl;
+      std::vector< VertexHandle > facet;
+      //VertexHandle curr_vh = curr_sh->first;
+      // CHECK ALL THE FACETS
+      for (std::vector<int>::iterator not_it = simplex.begin(); not_it != simplex.end(); ++not_it)
+        {
+          facet.clear();
+          //facet = {};
+          for (std::vector<int>::iterator it = simplex.begin(); it != simplex.end(); ++it)
+            if (it != not_it)
+              facet.push_back(*it); 
+          if (find(facet) == null_simplex())
+            return false;
+        } //endfor
+      return true;
+    }
+    
+    template <typename T>
+    void print_vector(std::vector<T> v)
+    {
+      std::cout << "[";
+      if (!v.empty())
+        {
+          std::cout << *(v.begin());
+          for (auto it = v.begin()+1; it != v.end(); ++it)
+            {
+              std::cout << ",";
+              std::cout << *it;
+            }
+      }
+      std::cout << "]";
+    }
+    
+    template <typename T>
+    void print_vvector(std::vector< std::vector <T> > vv)
+    {
+      std::cout << "[";
+      if (!vv.empty())
+        {
+          print_vector(*(vv.begin()));
+          for (auto it = vv.begin()+1; it != vv.end(); ++it)
+            {
+              std::cout << ",";
+              print_vector(*it);
+            }
+        }
+      std::cout << "]\n";
+    }
+
+  public:
+/**
+ * \brief Landmark choice strategy by iteratively adding the landmark the furthest from the
+ * current landmark set
+ * \arg W is the vector of points which will be the witnesses
+ * \arg nbP is the number of witnesses
+ * \arg nbL is the number of landmarks
+ * \arg WL is the matrix of the nearest landmarks with respect to witnesses (output)
+ */
+
+  template <typename KNearestNeighbours>
+    void landmark_choice_by_furthest_points(Point_Vector &W, int nbP, KNearestNeighbours &WL)
+  {
+    //std::cout << "Enter landmark_choice_by_furthest_points "<< std::endl;
+    //std::cout << "W="; print_vvector(W);
+    //double density = 5.;
+    Point_Vector wit_land_dist(nbP,std::vector<double>());    // distance matrix witness x landmarks
+    typeVectorVertex  chosen_landmarks;                       // landmark list
+
+    WL = KNearestNeighbours(nbP,std::vector<int>());                             
+    int current_number_of_landmarks=0;                        // counter for landmarks 
+    double curr_max_dist = 0;                                 // used for defining the furhest point from L
+    double curr_dist;                                         // used to stock the distance from the current point to L
+    double infty = std::numeric_limits<double>::infinity();   // infinity (see next entry)
+    std::vector< double > dist_to_L(nbP,infty);               // vector of current distances to L from points
+    // double mindist = infty;
+    int curr_max_w=0;                                         // the point currently furthest from L 
+    int j;
+    int temp_swap_int;                                        
+    double temp_swap_double;
+
+    //CHOICE OF THE FIRST LANDMARK
+    std::cout << "Enter the first landmark stage\n";
+    srand(354698);
+    int rand_int = rand()% nbP;
+    curr_max_w = rand_int; //For testing purposes a pseudo-random number is used here
+
+    for (current_number_of_landmarks = 0; current_number_of_landmarks != nbL; current_number_of_landmarks++)
+      {
+        //curr_max_w at this point is the next landmark
+        chosen_landmarks.push_back(curr_max_w);
+        //std::cout << "**********Entered loop with current number of landmarks = " << current_number_of_landmarks << std::endl;
+        //std::cout << "WL="; print_vvector(WL);
+        //std::cout << "WLD="; print_vvector(wit_land_dist);
+        //std::cout << "landmarks="; print_vector(chosen_landmarks); std::cout << std::endl;
+        for (auto v: WL)
+          v.push_back(current_number_of_landmarks);
+        for (int i = 0; i < nbP; ++i)
+          {
+            // iteration on points in W. update of distance vectors
+            
+            //std::cout << "In the loop with i=" << i << " and landmark=" << chosen_landmarks[current_number_of_landmarks] << std::endl;
+            //std::cout << "W[i]="; print_vector(W[i]); std::cout << " W[landmark]="; print_vector(W[chosen_landmarks[current_number_of_landmarks]]); std::cout << std::endl;
+            curr_dist = euclidean_distance(W[i],W[chosen_landmarks[current_number_of_landmarks]]);
+            //std::cout << "The problem is not in distance function\n";
+            wit_land_dist[i].push_back(curr_dist);
+            WL[i].push_back(current_number_of_landmarks);
+            //std::cout << "Push't back\n";
+            if (curr_dist < dist_to_L[i])
+              dist_to_L[i] = curr_dist;
+            j = current_number_of_landmarks;
+            //std::cout << "First half complete\n";
+            while (j > 0 && wit_land_dist[i][j-1] > wit_land_dist[i][j])
+              {
+                // sort the closest landmark vector for every witness
+                temp_swap_int = WL[i][j];
+                WL[i][j] = WL[i][j-1];
+                WL[i][j-1] = temp_swap_int;
+                temp_swap_double = wit_land_dist[i][j];
+                wit_land_dist[i][j] = wit_land_dist[i][j-1];
+                wit_land_dist[i][j-1] = temp_swap_double;
+                --j;
+              }
+            //std::cout << "result WL="; print_vvector(WL);
+            //std::cout << "result WLD="; print_vvector(wit_land_dist);
+            //std::cout << "result distL="; print_vector(dist_to_L); std::cout << std::endl;
+            //std::cout << "End loop\n";
+          }
+        //std::cout << "Distance to landmarks="; print_vector(dist_to_L); std::cout << std::endl;
+        curr_max_dist = 0;
+        for (int i = 0; i < nbP; ++i) {
+          if (dist_to_L[i] > curr_max_dist)
+            {
+              curr_max_dist = dist_to_L[i];
+              curr_max_w = i;
+            }
+        }
+        //std::cout << "Chose " << curr_max_w << " as new landmark\n";
+      }
+    //std::cout << endl;
+  }
+
+    /** \brief Landmark choice strategy by taking random vertices for landmarks.
+     *
+     */
+
+    // template <typename KNearestNeighbours>
+    // void landmark_choice_by_random_points(Point_Vector &W, int nbP, KNearestNeighbours &WL)
+    // {
+    //   std::cout << "Enter landmark_choice_by_random_points "<< std::endl;
+    //   //std::cout << "W="; print_vvector(W);
+    //   std::unordered_set< int >  chosen_landmarks;              // landmark set
+
+    //   Point_Vector wit_land_dist(nbP,std::vector<double>());    // distance matrix witness x landmarks
+
+    //   WL = KNearestNeighbours(nbP,std::vector<int>());                             
+    //   int current_number_of_landmarks=0;                        // counter for landmarks 
+
+    //   srand(24660);
+    //   int chosen_landmark = rand()%nbP;
+    //   double curr_dist;
+      
+    //   //int j;
+    //   //int temp_swap_int;                                        
+    //   //double temp_swap_double;
+
+      
+    //   for (current_number_of_landmarks = 0; current_number_of_landmarks != nbL; current_number_of_landmarks++)
+    //     {
+    //       while (chosen_landmarks.find(chosen_landmark) != chosen_landmarks.end())
+    //         {
+    //           srand((int)clock());
+    //           chosen_landmark = rand()% nbP;
+    //           //std::cout << chosen_landmark << "\n";
+    //         }
+    //       chosen_landmarks.insert(chosen_landmark);
+    //       //std::cout << "**********Entered loop with current number of landmarks = " << current_number_of_landmarks << std::endl;
+    //       //std::cout << "WL="; print_vvector(WL);
+    //       //std::cout << "WLD="; print_vvector(wit_land_dist);
+    //       //std::cout << "landmarks="; print_vector(chosen_landmarks); std::cout << std::endl;
+    //       for (auto v: WL)
+    //         v.push_back(current_number_of_landmarks);
+    //       for (int i = 0; i < nbP; ++i)
+    //         {
+    //           // iteration on points in W. update of distance vectors
+              
+    //           //std::cout << "In the loop with i=" << i << " and landmark=" << chosen_landmarks[current_number_of_landmarks] << std::endl;
+    //           //std::cout << "W[i]="; print_vector(W[i]); std::cout << " W[landmark]="; print_vector(W[chosen_landmarks[current_number_of_landmarks]]); std::cout << std::endl;
+    //           curr_dist = euclidean_distance(W[i],W[chosen_landmark]);
+    //           //std::cout << "The problem is not in distance function\n";
+    //           wit_land_dist[i].push_back(curr_dist);
+    //           WL[i].push_back(current_number_of_landmarks);
+    //           //std::cout << "Push't back\n";
+    //           //j = current_number_of_landmarks;
+    //           //std::cout << "First half complete\n";
+    //           //std::cout << "result WL="; print_vvector(WL);
+    //           //std::cout << "result WLD="; print_vvector(wit_land_dist);
+    //           //std::cout << "End loop\n";
+    //         }
+    //     }
+    //   for (int i = 0; i < nbP; i++)
+    //     {
+    //       sort(WL[i].begin(), WL[i].end(), [&](int j1, int j2){return wit_land_dist[i][j1] < wit_land_dist[i][j2];});
+    //     }
+    //   //std::cout << endl;
+    // }
+
+    /** \brief Landmark choice strategy by taking random vertices for landmarks.
+     *
+     */
+
+    //    template <typename KNearestNeighbours>
+    void landmark_choice_by_random_points(Point_Vector &W, int nbP, std::set<int> &L)
+    {
+      std::cout << "Enter landmark_choice_by_random_points "<< std::endl;
+      //std::cout << "W="; print_vvector(W);
+      //std::unordered_set< int >  chosen_landmarks;              // landmark set
+
+      //Point_Vector wit_land_dist(nbP,std::vector<double>());    // distance matrix witness x landmarks
+
+      //WL = KNearestNeighbours(nbP,std::vector<int>());                             
+      int current_number_of_landmarks=0;                        // counter for landmarks 
+
+      srand(24660);
+      int chosen_landmark = rand()%nbP;
+      //double curr_dist;
+      //int j;
+      //int temp_swap_int;                                        
+      //double temp_swap_double; 
+      for (current_number_of_landmarks = 0; current_number_of_landmarks != nbL; current_number_of_landmarks++)
+        {
+          while (L.find(chosen_landmark) != L.end())
+            {
+              srand((int)clock());
+              chosen_landmark = rand()% nbP;
+              //std::cout << chosen_landmark << "\n";
+            }
+          L.insert(chosen_landmark);
+          //std::cout << "**********Entered loop with current number of landmarks = " << current_number_of_landmarks << std::endl;
+          //std::cout << "WL="; print_vvector(WL);
+          //std::cout << "WLD="; print_vvector(wit_land_dist);
+          //std::cout << "landmarks="; print_vector(chosen_landmarks); std::cout << std::endl;
+          // for (auto v: WL)
+          //   v.push_back(current_number_of_landmarks);
+          // for (int i = 0; i < nbP; ++i)
+          //   {
+          //     // iteration on points in W. update of distance vectors
+              
+          //     //std::cout << "In the loop with i=" << i << " and landmark=" << chosen_landmarks[current_number_of_landmarks] << std::endl;
+          //     //std::cout << "W[i]="; print_vector(W[i]); std::cout << " W[landmark]="; print_vector(W[chosen_landmarks[current_number_of_landmarks]]); std::cout << std::endl;
+          //     curr_dist = euclidean_distance(W[i],W[chosen_landmark]);
+          //     //std::cout << "The problem is not in distance function\n";
+          //     wit_land_dist[i].push_back(curr_dist);
+          //     WL[i].push_back(current_number_of_landmarks);
+          //     //std::cout << "Push't back\n";
+          //     //j = current_number_of_landmarks;
+          //     //std::cout << "First half complete\n";
+          //     //std::cout << "result WL="; print_vvector(WL);
+          //     //std::cout << "result WLD="; print_vvector(wit_land_dist);
+          //     //std::cout << "End loop\n";
+          //   }
+        }
+      // for (int i = 0; i < nbP; i++)
+      //   {
+      //     sort(WL[i].begin(), WL[i].end(), [&](int j1, int j2){return wit_land_dist[i][j1] < wit_land_dist[i][j2];});
+      //   }
+      //std::cout << endl;
+    }
+
+    
+    /** \brief Construct the matrix |W|x(D+1) of D+1 closest landmarks
+     *  where W is the set of witnesses and D is the ambient dimension
+     */
+    template <typename KNearestNeighbours>
+    void nearest_landmarks(Point_Vector &W, std::set<int> &L, KNearestNeighbours &WL)
+    {
+      int D = W[0].size();
+      int nbP = W.size();
+      WL = KNearestNeighbours(nbP,std::vector<int>());
+      typedef std::pair<double,int> dist_i;
+      typedef bool (*comp)(dist_i,dist_i);
+      for (int W_i = 0; W_i < nbP; W_i++)
+        {
+          //std::cout << "<<<<<<<<<<<<<<" << W_i <<"\n"; 
+          std::priority_queue<dist_i, std::vector<dist_i>, comp> l_heap([&](dist_i j1, dist_i j2){return j1.first > j2.first;});
+          std::set<int>::iterator L_it;
+          int L_i;
+          for (L_it = L.begin(), L_i=0; L_it != L.end(); L_it++, L_i++)
+            {
+              dist_i dist = std::make_pair(euclidean_distance(W[W_i],W[*L_it]), L_i);
+              l_heap.push(dist);
+            }
+          for (int i = 0; i < D+1; i++)
+            {
+              dist_i dist = l_heap.top();
+              WL[W_i].push_back(dist.second);
+              //WL[W_i].insert(WL[W_i].begin(),dist.second);  
+              //std::cout << dist.first << " " << dist.second << std::endl;
+              l_heap.pop();
+            }
+        }
+    }
+    
+    /** \brief Search and output links around vertices that are not pseudomanifolds
+     *
+     */
+    void write_bad_links(std::ofstream& out_file)
+    {
+      out_file << "Bad links list\n";
+      std::cout << "Entered write_bad_links\n";
+      //typeVectorVertex testv = {9,15,17};
+      //int count = 0;
+      for (auto v: complex_vertex_range())
+        {
+          //std::cout << "Vertex " << v << ":\n";
+          std::vector< Vertex_handle > link_vertices;
+          // Fill link_vertices
+          for (auto u: complex_vertex_range())
+            {
+              typeVectorVertex edge = {u,v};
+              if (u != v && find(edge) != null_simplex())   
+                link_vertices.push_back(u);
+            }
+          /*
+            print_vector(link_vertices);
+            std::cout << "\n";
+          */
+          // Find the dimension
+          typeVectorVertex empty_simplex = {};
+          int d = link_dim(link_vertices, link_vertices.begin(),-1, empty_simplex);
+          //std::cout << " dim " << d << "\n";
+          //Siblings* curr_sibl = root();
+          if (link_is_pseudomanifold(link_vertices,d))
+            count_good[d]++;
+          //out_file << "Bad link at " << v << "\n";          
+        }
+      //out_file << "Number of bad links: " << count << "/" << root()->size();
+      //std::cout << "Number of bad links: " << count << "/" << root()->size() << std::endl;
+      nc = nbL;
+      for (unsigned int i = 0; i != count_good.size(); i++)
+	{
+	  out_file << "count_good[" << i << "] = " << count_good[i] << std::endl;
+	  nc -= count_good[i];
+	  if (count_good[i] != 0)
+	    std::cout << "count_good[" << i << "] = " << count_good[i] << std::endl;
+	}
+      for (unsigned int i = 0; i != count_bad.size(); i++)
+	{
+	  out_file << "count_bad[" << i << "] = " << count_bad[i] << std::endl;
+	  nc -= count_bad[i];
+	  if (count_bad[i] != 0)
+	    std::cout << "count_bad[" << i << "] = " << count_bad[i] << std::endl;
+	}
+      std::cout << "not_connected = " << nc << std::endl;
+    }
+
+  private:
+
+  std::vector<int> count_good;
+  std::vector<int> count_bad;
+  int nc;
+
+    int link_dim(std::vector< Vertex_handle >& link_vertices,
+                 typename std::vector< Vertex_handle >::iterator curr_v,
+                 int curr_d,
+                 typeVectorVertex& curr_simplex)
+    {
+      //std::cout << "Entered link_dim for " << *(curr_v-1) << "\n";
+      Simplex_handle sh;
+      int final_d = curr_d;
+      typename std::vector< Vertex_handle >::iterator it;
+      for (it = curr_v; it != link_vertices.end(); ++it)
+        {
+          curr_simplex.push_back(*it);
+          /*
+          std::cout << "Searching for ";
+          print_vector(curr_simplex);
+          std::cout << " curr_dim " << curr_d << " final_dim " << final_d;
+          */
+          sh = find(curr_simplex);
+          if (sh != null_simplex())
+            {
+              //std::cout << " -> " << *it << "\n";
+              int d = link_dim(link_vertices, it+1, curr_d+1, curr_simplex);
+              if (d > final_d)
+                final_d = d;
+            }
+          /*
+          else
+            std::cout << "\n";
+          */
+          curr_simplex.pop_back();
+        }
+      return final_d;
+    }
+
+    // color is false is a (d-1)-dim face, true is a d-dim face 
+    //typedef bool Color;
+    // graph is an adjacency list
+    typedef typename boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> Adj_graph;
+    // map that gives to a certain simplex its node in graph and its dimension
+    //typedef std::pair<boost::vecS,Color> Reference;
+    typedef boost::graph_traits<Adj_graph>::vertex_descriptor Vertex_t;
+    typedef boost::graph_traits<Adj_graph>::edge_descriptor Edge_t;
+
+    typedef boost::container::flat_map<Simplex_handle, Vertex_t> Graph_map;
+    
+    /* \brief Verifies if the simplices formed by vertices given by link_vertices 
+     * form a pseudomanifold.
+     * The idea is to make a bipartite graph, where vertices are the d- and (d-1)-dimensional
+     * faces and edges represent adjacency between them.
+     */
+    bool link_is_pseudomanifold(std::vector< Vertex_handle >& link_vertices,
+                                int dimension)
+    {
+      Adj_graph adj_graph;
+      Graph_map d_map, f_map; // d_map = map for d-dimensional simplices
+                              // f_map = map for its facets
+      typeVectorVertex empty_vector = {};
+      add_vertices(link_vertices,
+                   link_vertices.begin(),
+                   adj_graph,
+                   d_map,
+                   f_map,
+                   empty_vector,
+                   0, dimension);
+      //std::cout << "DMAP_SIZE: " << d_map.size() << "\n";
+      //std::cout << "FMAP_SIZE: " << f_map.size() << "\n";
+      add_edges(adj_graph, d_map, f_map);
+      for (auto f_map_it : f_map)
+        {
+          //std::cout << "Degree of " << f_map_it.first->first << " is " << boost::out_degree(f_map_it.second, adj_graph) << "\n";
+          if (boost::out_degree(f_map_it.second, adj_graph) != 2)
+	    {
+	      count_bad[dimension]++;
+	      return false;
+	    }
+        }
+      // At this point I know that all (d-1)-simplices are adjacent to exactly 2 d-simplices
+      // What is left is to check the connexity
+      std::vector<int> components(boost::num_vertices(adj_graph));
+      return (boost::connected_components(adj_graph, &components[0]) == 1);
+    }
+
+    void add_vertices(typeVectorVertex& link_vertices,
+                      typename typeVectorVertex::iterator curr_v,
+                      Adj_graph& adj_graph,
+                      Graph_map& d_map,
+                      Graph_map& f_map,
+                      typeVectorVertex& curr_simplex,
+                      int curr_d,
+                      int dimension)
+    {
+      Simplex_handle sh;
+      Vertex_t vert;
+      typename typeVectorVertex::iterator it;
+      std::pair<typename Graph_map::iterator,bool> resPair;
+      //typename Graph_map::iterator resPair;
+      //Add vertices
+      //std::cout << "Entered add vertices\n";
+      for (it = curr_v; it != link_vertices.end(); ++it)
+        {
+          curr_simplex.push_back(*it);
+          /*
+          std::cout << "Searching for ";
+          print_vector(curr_simplex);
+          std::cout << " curr_dim " << curr_d << " d " << dimension << "";
+          */
+          sh = find(curr_simplex);
+          if (sh != null_simplex())
+            {
+              //std::cout << " added\n";
+              if (curr_d == dimension)
+                {
+                  vert = boost::add_vertex(adj_graph);
+                  resPair = d_map.emplace(sh,vert);
+                }
+              else
+                {
+                  if (curr_d == dimension-1)
+                    {
+                      vert = boost::add_vertex(adj_graph);
+                      resPair = f_map.emplace(sh,vert);
+                    }
+                  add_vertices(link_vertices,
+                               it+1,
+                               adj_graph,
+                               d_map,
+                               f_map,
+                               curr_simplex,
+                               curr_d+1, dimension);
+                }
+            }
+          /*
+          else
+            std::cout << "\n";
+          */
+          curr_simplex.pop_back();
+        }
+    }
+    
+    void add_edges(Adj_graph& adj_graph,
+                   Graph_map& d_map,
+                   Graph_map& f_map)
+    {
+      Simplex_handle sh;
+      // Add edges
+      //std::cout << "Entered add edges:\n";
+      typename Graph_map::iterator map_it;
+      for (auto d_map_pair : d_map)
+        {
+          //std::cout << "*";
+          sh = d_map_pair.first;
+          Vertex_t d_vert = d_map_pair.second;
+          for (auto facet_sh : boundary_simplex_range(sh))
+            //for (auto f_map_it : f_map)
+            {
+              //std::cout << "'"; 
+              map_it = f_map.find(facet_sh);
+              //We must have all the facets in the graph at this point
+              assert(map_it != f_map.end());
+              Vertex_t f_vert = map_it->second;
+              //std::cout << "Added edge " << sh->first << "-" << map_it->first->first << "\n";
+              boost::add_edge(d_vert,f_vert,adj_graph);
+            }
+        }
+    }
+
+    //////////////////////////////////////////////////////////////////////////////////////////////////
+    //***********COLLAPSES**************************************************************************//
+    //////////////////////////////////////////////////////////////////////////////////////////////////
+
+    
+
+
+
+
+    
+}; //class Witness_complex
+
+
+  
+} // namespace Guhdi
+
+#endif