#if defined (_MSC_VER) && !defined (_WIN64)
#pragma warning(disable:4244) // boost::number_distance::distance()
                              // converts 64 to 32 bits integers
#endif

#include "Cluster_classification.h"
#include "Color_ramp.h"

#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Delaunay_triangulation_cell_base_3.h>
#include <CGAL/Triangulation_vertex_base_with_info_3.h>
#include <CGAL/Timer.h>
#include <CGAL/Memory_sizer.h>

#include <QLineEdit>

#include <CGAL/Three/Viewer_interface.h>

#include <set>
#include <stack>
#include <algorithm>
#include <boost/array.hpp>

Cluster_classification::Cluster_classification(Scene_points_with_normal_item* points)
  : m_points (points)
  , m_input_is_las (false)
{
  m_index_color = 1;

  reset_indices();

  backup_existing_colors_and_add_new();

  bool cluster_found = false;
  boost::tie (m_cluster_id, cluster_found) = m_points->point_set()->property_map<int>("shape");
  if (!cluster_found)
  {
    std::cerr << "Error! Cluster not found!" << std::endl;
    abort();
  }

  CGAL::Classification::create_clusters_from_indices (*(m_points->point_set()),
                                                      m_points->point_set()->point_map(),
                                                      m_cluster_id,
                                                      m_clusters);

  std::cerr << m_clusters.size() << " cluster(s) found" << std::endl;

  bool training_found = false;
  boost::tie (m_training, training_found) = m_points->point_set()->add_property_map<int>("training", -1);
  bool classif_found = false;
  boost::tie (m_classif, classif_found) = m_points->point_set()->add_property_map<int>("label", -1);

  training_found = !training_found; // add_property_map returns false if
  classif_found = !classif_found;   // property was already there

  bool las_found = false;

  if (!classif_found)
  {
    Point_set::Property_map<unsigned char> las_classif;
    boost::tie (las_classif, las_found) = m_points->point_set()->property_map<unsigned char>("classification");
    if (las_found)
    {
      m_input_is_las = true;
      for (Point_set::const_iterator it = m_points->point_set()->begin();
           it != m_points->point_set()->first_selected(); ++ it)
      {
        unsigned char uc = las_classif[*it];
        m_classif[*it] = int(uc);
        if (!training_found)
          m_training[*it] = int(uc);
      }
      m_points->point_set()->remove_property_map (las_classif);
      classif_found = true;
      training_found = true;
    }
  }

  if (training_found || classif_found)
  {
    std::vector<int> used_indices;

    for (Point_set::const_iterator it = m_points->point_set()->begin();
         it != m_points->point_set()->first_selected(); ++ it)
    {
      if (training_found)
      {
        int l = m_training[*it];
        if (l >= 0)
        {
          if (std::size_t(l) >= used_indices.size())
            used_indices.resize(std::size_t(l + 1), -1);
          used_indices[std::size_t(l)] = 0;
        }
      }
      if (classif_found)
      {
        int l = m_classif[*it];
        if (l >= 0)
        {
          if (std::size_t(l) >= used_indices.size())
            used_indices.resize(std::size_t(l + 1), -1);
          used_indices[std::size_t(l)] = 0;
        }
      }
    }

    // map indices to filtered indices
    int current_idx = 0;
    for (std::size_t i = 0; i < used_indices.size(); ++ i)
    {
      if (las_found && (i < 2))
      {
        used_indices[i] = -1;
        continue;
      }
      if (used_indices[i] == -1)
        continue;

      used_indices[i] = current_idx;
      ++ current_idx;
    }

    for (Point_set::const_iterator it = m_points->point_set()->begin();
         it != m_points->point_set()->first_selected(); ++ it)
    {
      int c = m_cluster_id[*it];

      if (training_found)
      {
        if (std::size_t(current_idx) != used_indices.size()) // Empty indices -> reorder indices in point set
        {
          if (las_found && (m_training[*it] == 0 || m_training[*it] == 1)) // Unclassified class in LAS
            m_training[*it] = -1;
          else if (m_training[*it] != -1)
            m_training[*it] = used_indices[std::size_t(m_training[*it])];
        }
        if (c != -1 && m_training[*it] != -1)
          m_clusters[c].training() = m_training[*it];
      }
      if (classif_found)
      {
        if (std::size_t(current_idx) != used_indices.size()) // Empty indices -> reorder indices in point set
        {
          if (las_found && (m_classif[*it] == 0 || m_classif[*it] == 1)) // Unclassified class in LAS
            m_classif[*it] = -1;
          else if (m_classif[*it] != -1)
            m_classif[*it] = used_indices[std::size_t(m_classif[*it])];
        }
        if (c != -1 && m_classif[*it] != -1)
          m_clusters[c].label() = m_classif[*it];
      }
    }

    std::map<int, std::string> label_names;
    if (las_found) // Use LAS standard
    {
      // label_names.insert (std::make_pair (0, std::string("never_clfied")));
      // label_names.insert (std::make_pair (1, std::string("unclassified")));
      label_names.insert (std::make_pair (2, std::string("ground")));
      label_names.insert (std::make_pair (3, std::string("low_veget")));
      label_names.insert (std::make_pair (4, std::string("med_veget")));
      label_names.insert (std::make_pair (5, std::string("high_veget")));
      label_names.insert (std::make_pair (6, std::string("building")));
      label_names.insert (std::make_pair (7, std::string("noise")));
      label_names.insert (std::make_pair (8, std::string("reserved")));
      label_names.insert (std::make_pair (9, std::string("water")));
      label_names.insert (std::make_pair (10, std::string("rail")));
      label_names.insert (std::make_pair (11, std::string("road_surface")));
      label_names.insert (std::make_pair (12, std::string("reserved_2")));
      label_names.insert (std::make_pair (13, std::string("wire_guard")));
      label_names.insert (std::make_pair (14, std::string("wire_conduct")));
      label_names.insert (std::make_pair (15, std::string("trans_tower")));
      label_names.insert (std::make_pair (16, std::string("wire_connect")));
      label_names.insert (std::make_pair (17, std::string("bridge_deck")));
      label_names.insert (std::make_pair (18, std::string("high_noise")));
    }
    else // Try to recover label names from PLY comments
    {

      const std::string& comments = m_points->comments();
      std::istringstream stream (comments);
      std::string line;
      while (getline(stream, line))
      {
        std::stringstream iss (line);
        std::string tag;
        if (iss >> tag && tag == "label")
        {
          int idx;
          std::string name;
          if (iss >> idx >> name)
            label_names.insert (std::make_pair (idx, name));
        }
      }
    }

    for (std::size_t i = 0; i < used_indices.size(); ++ i)
    {
      if (used_indices[i] == -1)
        continue;

      Label_handle new_label;
      std::map<int, std::string>::iterator found
        = label_names.find (int(i));
      if (found != label_names.end())
        new_label = m_labels.add(found->second.c_str());
      else
      {
        std::ostringstream oss;
        oss << "label_" << i;
        new_label = m_labels.add(oss.str().c_str());
      }
    }
  }
  else
  {
    m_labels.add("ground");
    m_labels.add("vegetation");
    m_labels.add("roof");
    m_labels.add("facade");
  }

  update_comments_of_point_set_item();

  m_sowf = new Sum_of_weighted_features (m_labels, m_features);
  m_ethz = nullptr;
#ifdef CGAL_LINKED_WITH_OPENCV
  m_random_forest = nullptr;
#endif

  // Compute neighborhood
#if 0
  typedef CGAL::Triangulation_vertex_base_with_info_3<int, Kernel>         Vb;
  typedef CGAL::Delaunay_triangulation_cell_base_3<Kernel>                 Cb;
  typedef CGAL::Triangulation_data_structure_3<Vb, Cb>                     Tds;
  typedef CGAL::Delaunay_triangulation_3<Kernel, Tds>                      Delaunay;

  Delaunay dt (boost::make_transform_iterator
               (m_points->point_set()->begin(),
                Point_set_with_cluster_info (m_points->point_set(),
                                             m_cluster_id)),
                boost::make_transform_iterator
               (m_points->point_set()->end(),
                Point_set_with_cluster_info (m_points->point_set(),
                                             m_cluster_id)));

  std::set<std::pair<int, int> > adjacencies;

  for (Delaunay::Finite_edges_iterator it = dt.finite_edges_begin();
       it != dt.finite_edges_end(); ++ it)
  {
    Delaunay::Vertex_handle v0 = it->first->vertex (it->second);
    Delaunay::Vertex_handle v1 = it->first->vertex (it->third);
    int a = v0->info();
    int b = v1->info();

    if (a == -1 || b == -1)
      continue;

    if (a > b)
      std::swap (a, b);

    adjacencies.insert (std::make_pair (a, b));
  }

  for (std::set<std::pair<int, int> >::iterator it = adjacencies.begin();
       it != adjacencies.end(); ++ it)
  {
    m_clusters[std::size_t(it->first)].neighbors->push_back (std::size_t(it->second));
    m_clusters[std::size_t(it->second)].neighbors->push_back (std::size_t(it->first));
  }
#endif
}

Cluster_classification::~Cluster_classification()
{
  if (m_sowf != nullptr)
    delete m_sowf;
  if (m_ethz != nullptr)
    delete m_ethz;
#ifdef CGAL_LINKED_WITH_OPENCV
  if (m_random_forest != nullptr)
    delete m_random_forest;
#endif
#ifdef CGAL_LINKED_WITH_TENSORFLOW
  if (m_neural_network != nullptr)
    delete m_neural_network;
#endif
  if (m_points != nullptr)
  {
    for (Point_set::const_iterator it = m_points->point_set()->begin();
         it != m_points->point_set()->first_selected(); ++ it)
    {
      int c = m_cluster_id[*it];
      if (c != -1)
      {
        m_training[*it] = m_clusters[c].training();
        m_classif[*it] = m_clusters[c].label();
      }
      else
      {
        m_training[*it] = -1;
        m_classif[*it] = -1;
      }
    }

    // For LAS saving, convert classification info in the LAS standard
//    if (m_input_is_las)
    {
      Point_set::Property_map<unsigned char> las_classif
        = m_points->point_set()->add_property_map<unsigned char>("classification", 0).first;

      std::vector<unsigned char> label_indices;

      unsigned char custom = 19;
      for (std::size_t i = 0; i < m_labels.size(); ++ i)
      {
        if (m_labels[i]->name() == "ground")
          label_indices.push_back (2);
        else if (m_labels[i]->name() == "low_veget")
          label_indices.push_back (3);
        else if (m_labels[i]->name() == "med_veget" || m_labels[i]->name() == "vegetation")
          label_indices.push_back (4);
        else if (m_labels[i]->name() == "high_veget")
          label_indices.push_back (5);
        else if (m_labels[i]->name() == "building" || m_labels[i]->name() == "roof")
          label_indices.push_back (6);
        else if (m_labels[i]->name() == "noise")
          label_indices.push_back (7);
        else if (m_labels[i]->name() == "reserved" || m_labels[i]->name() == "facade")
          label_indices.push_back (8);
        else if (m_labels[i]->name() == "water")
          label_indices.push_back (9);
        else if (m_labels[i]->name() == "rail")
          label_indices.push_back (10);
        else if (m_labels[i]->name() == "road_surface")
          label_indices.push_back (11);
        else if (m_labels[i]->name() == "reserved_2")
          label_indices.push_back (12);
        else if (m_labels[i]->name() == "wire_guard")
          label_indices.push_back (13);
        else if (m_labels[i]->name() == "wire_conduct")
          label_indices.push_back (14);
        else if (m_labels[i]->name() == "trans_tower")
          label_indices.push_back (15);
        else if (m_labels[i]->name() == "wire_connect")
          label_indices.push_back (16);
        else if (m_labels[i]->name() == "bridge_deck")
          label_indices.push_back (17);
        else if (m_labels[i]->name() == "high_noise")
          label_indices.push_back (18);
        else
          label_indices.push_back (custom ++);
      }

      for (Point_set::const_iterator it = m_points->point_set()->begin();
           it != m_points->point_set()->end(); ++ it)
      {
        int c = m_classif[*it];
        unsigned char lc = 1; // unclassified in LAS standard
        if (c != -1)
          lc = label_indices[std::size_t(c)];

        las_classif[*it] = lc;

        int t = m_training[*it];
        unsigned char lt = 1; // unclassified in LAS standard
        if (t != -1)
          lt = label_indices[std::size_t(t)];

        m_training[*it] = int(lt);
      }

      m_points->point_set()->remove_property_map (m_classif);
    }

    reset_colors();
    erase_item();
  }
}

void Cluster_classification::backup_existing_colors_and_add_new()
{
  if (m_points->point_set()->has_colors())
  {
    m_color = m_points->point_set()->add_property_map<CGAL::IO::Color>("real_color").first;
    for (Point_set::const_iterator it = m_points->point_set()->begin();
         it != m_points->point_set()->first_selected(); ++ it)
      m_color[*it] = CGAL::IO::Color ((unsigned char)(255 * m_points->point_set()->red(*it)),
                                  (unsigned char)(255 * m_points->point_set()->green(*it)),
                                  (unsigned char)(255 * m_points->point_set()->blue(*it)));

    m_points->point_set()->remove_colors();
  }

  m_points->point_set()->add_colors();
}

void Cluster_classification::reset_colors()
{
  if (m_color == Point_set::Property_map<CGAL::IO::Color>())
    m_points->point_set()->remove_colors();
  else
  {
    for (Point_set::const_iterator it = m_points->point_set()->begin();
         it != m_points->point_set()->first_selected(); ++ it)
      m_points->point_set()->set_color(*it, m_color[*it]);

    m_points->point_set()->remove_property_map(m_color);
  }
}

void Cluster_classification::change_color (int index, float* vmin, float* vmax)
{
  m_index_color = index;

  int index_color = real_index_color();

  // Colors
  static Color_ramp ramp;
  ramp.build_rainbow();
  reset_indices();

  if (index_color == -1) // item color
    m_points->point_set()->remove_colors();
  else
  {
    if (!m_points->point_set()->has_colors())
      m_points->point_set()->add_colors();

    if (index_color == 0) // real colors
    {

      for (Point_set::const_iterator it = m_points->point_set()->begin();
           it != m_points->point_set()->first_selected(); ++ it)
        m_points->point_set()->set_color(*it, m_color[*it]);
    }
    else if (index_color == 1) // classif
    {
      for (Point_set::const_iterator it = m_points->point_set()->begin();
           it != m_points->point_set()->first_selected(); ++ it)
      {
        QColor color (0, 0, 0);
        int cid = m_cluster_id[*it];
        if (cid != -1)
        {
          std::size_t c = m_clusters[cid].label();

          if (c != std::size_t(-1))
            color = label_qcolor (m_labels[std::size_t(c)]);
        }

        m_points->point_set()->set_color(*it, color);
      }
    }
    else if (index_color == 2) // training
    {
      for (Point_set::const_iterator it = m_points->point_set()->begin();
           it != m_points->point_set()->first_selected(); ++ it)
      {
        QColor color (0, 0, 0);
        int cid = m_cluster_id[*it];
        float div = 1;

        if (cid != -1)
        {
          int c = m_clusters[cid].training();
          int c2 = m_clusters[cid].label();

          if (c != -1)
            color = label_qcolor (m_labels[std::size_t(c)]);

          if (c != c2)
            div = 2;
        }
        m_points->point_set()->set_color(*it, color.red() / div, color.green() / div, color.blue() / div);
      }
    }
    else if (index_color == 3) // clusters
    {
      for (Point_set::const_iterator it = m_points->point_set()->begin();
           it != m_points->point_set()->first_selected(); ++ it)
      {
        int cid = m_cluster_id[*it];

        if (cid != -1)
        {
          srand(cid);
          m_points->point_set()->set_color(*it, 64 + rand() % 192, 64 + rand() % 192, 64 + rand() % 192);
        }
        else
        {
          m_points->point_set()->set_color(*it);
        }
      }
    }
    else
    {
      std::size_t corrected_index = index_color - 4;
      if (corrected_index < m_labels.size()) // Display label probabilities
      {
        if (m_label_probabilities.size() <= corrected_index ||
            m_label_probabilities[corrected_index].size() != m_clusters.size())
        {
          for (Point_set::const_iterator it = m_points->point_set()->begin();
               it != m_points->point_set()->first_selected(); ++ it)
            m_points->point_set()->set_color(*it);
        }
        else
        {
          for (Point_set::const_iterator it = m_points->point_set()->begin();
               it != m_points->point_set()->first_selected(); ++ it)
          {
            int cid = m_cluster_id[*it];
            if (cid != -1)
            {
              float v = (std::max) (0.f, (std::min)(1.f, m_label_probabilities[corrected_index][cid]));
              m_points->point_set()->set_color(*it, ramp.r(v) * 255, ramp.g(v) * 255, ramp.b(v) * 255);
            }
            else
              m_points->point_set()->set_color(*it);
          }
        }
      }
      else
      {
        corrected_index -= m_labels.size();
        if (corrected_index >= m_features.size())
        {
          std::cerr << "Error: trying to access feature " << corrected_index << " out of " << m_features.size() << std::endl;
          return;
        }

        Feature_handle feature = m_features[corrected_index];

        float min = (std::numeric_limits<float>::max)();
        float max = -(std::numeric_limits<float>::max)();

        if (vmin != nullptr && vmax != nullptr
            && *vmin != std::numeric_limits<float>::infinity()
            && *vmax != std::numeric_limits<float>::infinity())
        {
          min = *vmin;
          max = *vmax;
        }
        else
        {
          for (Point_set::const_iterator it = m_points->point_set()->begin();
               it != m_points->point_set()->first_selected(); ++ it)
          {
            int cid = m_cluster_id[*it];
            if (cid != -1)
            {
              if (feature->value(cid) > max)
                max = feature->value(cid);
              if (feature->value(cid) < min)
                min = feature->value(cid);
            }
          }
        }

        for (Point_set::const_iterator it = m_points->point_set()->begin();
             it != m_points->point_set()->first_selected(); ++ it)
        {
          int cid = m_cluster_id[*it];
          if (cid != -1)
          {
            float v = (feature->value(cid) - min) / (max - min);
            if (v < 0.f) v = 0.f;
            if (v > 1.f) v = 1.f;

            m_points->point_set()->set_color(*it, ramp.r(v) * 255, ramp.g(v) * 255, ramp.b(v) * 255);
          }
          else
            m_points->point_set()->set_color(*it);
        }

        if (vmin != nullptr && vmax != nullptr)
        {
          *vmin = min;
          *vmax = max;
        }
      }
    }
  }

  for (Point_set::const_iterator it = m_points->point_set()->first_selected();
       it != m_points->point_set()->end(); ++ it)
    m_points->point_set()->set_color(*it, 255, 0, 0);
}

int Cluster_classification::real_index_color() const
{
  int out = m_index_color;

  if (out == 0 && m_color == Point_set::Property_map<CGAL::IO::Color>())
    out = -1;
  return out;
}

void Cluster_classification::reset_indices ()
{
  Point_set::Property_map<Point_set::Index> indices
    = m_points->point_set()->property_map<Point_set::Index>("index").first;

  m_points->point_set()->unselect_all();
  Point_set::Index idx;
  ++ idx;
  for (std::size_t i = 0; i < m_points->point_set()->size(); ++ i)
    *(indices.begin() + i) = idx ++;
}

void Cluster_classification::compute_features (std::size_t nb_scales, float voxel_size)
{
  CGAL_assertion (!(m_points->point_set()->empty()));

  reset_indices();

  std::cerr << "Computing pointwise features with " << nb_scales << " scale(s) and ";
  if (voxel_size == -1)
    std::cerr << "automatic voxel size" << std::endl;
  else
    std::cerr << "voxel size = " << voxel_size << std::endl;

  m_features.clear();

  Point_set::Vector_map normal_map;
  bool normals = m_points->point_set()->has_normal_map();
  if (normals)
    normal_map = m_points->point_set()->normal_map();

  bool colors = (m_color != Point_set::Property_map<CGAL::IO::Color>());

  Point_set::Property_map<boost::uint8_t> echo_map;
  bool echo;
  boost::tie (echo_map, echo) = m_points->point_set()->template property_map<boost::uint8_t>("echo");
  if (!echo)
    boost::tie (echo_map, echo) = m_points->point_set()->template property_map<boost::uint8_t>("number_of_returns");

  Feature_set pointwise_features;

  Generator generator (*(m_points->point_set()), m_points->point_set()->point_map(), nb_scales, voxel_size);

  CGAL::Real_timer t;
  t.start();

#ifdef CGAL_LINKED_WITH_TBB
  pointwise_features.begin_parallel_additions();
#endif

  generator.generate_point_based_features(pointwise_features);
  if (normals)
    generator.generate_normal_based_features (pointwise_features, normal_map);
  if (colors)
    generator.generate_color_based_features (pointwise_features, m_color);
  if (echo)
    generator.generate_echo_based_features (pointwise_features, echo_map);

#ifdef CGAL_LINKED_WITH_TBB
  pointwise_features.end_parallel_additions();
#endif

  add_remaining_point_set_properties_as_features(pointwise_features);

  t.stop();
  std::cerr << pointwise_features.size() << " feature(s) computed in " << t.time() << " second(s)" << std::endl;
  t.reset();

  std::cerr << "Computing cluster features" << std::endl;
  t.start();

#ifdef CGAL_LINKED_WITH_TBB
  m_features.begin_parallel_additions();
#endif

  for (std::size_t i = 0; i < pointwise_features.size(); ++ i)
  {
    m_features.template add<Mean_of_feature> (m_clusters,
                                              pointwise_features[i]);
  }

#ifdef CGAL_LINKED_WITH_TBB
  m_features.end_parallel_additions();
  m_features.begin_parallel_additions();
#endif

  for (std::size_t i = 0; i < pointwise_features.size(); ++ i)
  {
    m_features.template add<Variance_of_feature> (m_clusters,
                                                  pointwise_features[i],
                                                  m_features[i]);
  }

  add_cluster_features();

#ifdef CGAL_LINKED_WITH_TBB
  m_features.end_parallel_additions();
#endif

  t.stop();
  std::cerr << m_features.size() << " feature(s) computed in " << t.time() << " second(s)" << std::endl;

  delete m_sowf;
  m_sowf = new Sum_of_weighted_features (m_labels, m_features);
  if (m_ethz != nullptr)
  {
    delete m_ethz;
    m_ethz = nullptr;
  }
#ifdef CGAL_LINKED_WITH_OPENCV
  if (m_random_forest != nullptr)
  {
    delete m_random_forest;
    m_random_forest = nullptr;
  }
#endif

  std::cerr << "Features = " << m_features.size() << std::endl;
}

void Cluster_classification::select_random_region()
{
  std::size_t c = rand() % m_clusters.size();
  std::set<Point_set::Index> in_cluster;
  for (std::size_t i = 0; i < m_clusters[c].size(); ++ i)
    in_cluster.insert (m_clusters[c].index(i));

  std::vector<std::size_t> selected;
  std::vector<std::size_t> unselected;

  for (Point_set::const_iterator it = m_points->point_set()->begin();
       it != m_points->point_set()->end(); ++ it)
    if (in_cluster.find (*it) != in_cluster.end())
      selected.push_back (*it);
    else
      unselected.push_back (*it);

  for (std::size_t i = 0; i < unselected.size(); ++ i)
    *(m_points->point_set()->begin() + i) = unselected[i];
  for (std::size_t i = 0; i < selected.size(); ++ i)
    *(m_points->point_set()->begin() + (unselected.size() + i)) = selected[i];

  m_points->point_set()->set_first_selected
    (m_points->point_set()->begin() + unselected.size());

}

void Cluster_classification::add_remaining_point_set_properties_as_features(Feature_set& feature_set)
{
  const std::vector<std::string>& prop = m_points->point_set()->base().properties();

  for (std::size_t i = 0; i < prop.size(); ++ i)
  {
    if (prop[i] == "index" ||
        prop[i] == "point" ||
        prop[i] == "normal" ||
        prop[i] == "echo" ||
        prop[i] == "number_of_returns" ||
        prop[i] == "training" ||
        prop[i] == "label" ||
        prop[i] == "classification" ||
        prop[i] == "scan_direction_flag" ||
        prop[i] == "real_color" ||
        prop[i] == "shape" ||
        prop[i] == "red" || prop[i] == "green" || prop[i] == "blue" ||
        prop[i] == "r" || prop[i] == "g" || prop[i] == "b")
      continue;

    if (try_adding_simple_feature<boost::int8_t>(feature_set, prop[i]))
      continue;
    if (try_adding_simple_feature<boost::uint8_t>(feature_set, prop[i]))
      continue;
    if (try_adding_simple_feature<boost::int16_t>(feature_set, prop[i]))
      continue;
    if (try_adding_simple_feature<boost::uint16_t>(feature_set, prop[i]))
      continue;
    if (try_adding_simple_feature<boost::int32_t>(feature_set, prop[i]))
      continue;
    if (try_adding_simple_feature<boost::uint32_t>(feature_set, prop[i]))
      continue;
    if (try_adding_simple_feature<float>(feature_set, prop[i]))
      continue;
    if (try_adding_simple_feature<double>(feature_set, prop[i]))
      continue;
  }
}

void Cluster_classification::train(int classifier, const QMultipleInputDialog& dialog)
{
  if (m_features.size() == 0)
  {
    std::cerr << "Error: features not computed" << std::endl;
    return;
  }
  reset_indices();

  m_label_probabilities.clear();
  m_label_probabilities.resize (m_labels.size());
  for (std::size_t i = 0; i < m_label_probabilities.size(); ++ i)
    m_label_probabilities[i].resize (m_clusters.size(), -1);

  std::vector<std::size_t> nb_label (m_labels.size(), 0);
  std::size_t nb_total = 0;

  std::vector<int> training;
  training.reserve (m_clusters.size());
  for (std::size_t i = 0; i < m_clusters.size(); ++ i)
  {
    training.push_back (m_clusters[i].training());
    if (training.back() != -1)
    {
      nb_label[std::size_t(training.back())] ++;
      ++ nb_total;
    }
  }

  std::cerr << nb_total << " cluster(s) used for training ("
            << 100. * (nb_total / double(m_clusters.size())) << "% of the total):" << std::endl;
  for (std::size_t i = 0; i < m_labels.size(); ++ i)
    std::cerr << " * " << m_labels[i]->name() << ": " << nb_label[i] << " clusters(s)" << std::endl;

  std::vector<int> indices (m_clusters.size(), -1);

  if (classifier == CGAL_CLASSIFICATION_SOWF_NUMBER)
  {
    m_sowf->train<Concurrency_tag>(training, dialog.get<QSpinBox>("trials")->value());
    CGAL::Classification::classify<Concurrency_tag> (m_clusters,
                                                     m_labels, *m_sowf,
                                                     indices, m_label_probabilities);
  }
  else if (classifier == CGAL_CLASSIFICATION_ETHZ_NUMBER)
  {
    if (m_ethz != nullptr)
      delete m_ethz;
    m_ethz = new ETHZ_random_forest (m_labels, m_features);
    m_ethz->train<Concurrency_tag>(training, true,
                                   dialog.get<QSpinBox>("num_trees")->value(),
                                   dialog.get<QSpinBox>("max_depth")->value());
    CGAL::Classification::classify<Concurrency_tag> (m_clusters,
                                                     m_labels, *m_ethz,
                                                     indices, m_label_probabilities);
  }
  else if (classifier == CGAL_CLASSIFICATION_OPENCV_NUMBER)
  {
#ifdef CGAL_LINKED_WITH_OPENCV
    if (m_random_forest != nullptr)
      delete m_random_forest;
    m_random_forest = new Random_forest (m_labels, m_features,
                                         dialog.get<QSpinBox>("max_depth")->value(), 5, 15,
                                         dialog.get<QSpinBox>("num_trees")->value());
    m_random_forest->train (training);
    CGAL::Classification::classify<Concurrency_tag> (m_clusters,
                                                     m_labels, *m_random_forest,
                                                     indices, m_label_probabilities);
#endif
  }

  for (std::size_t i = 0; i < m_clusters.size(); ++ i)
    m_clusters[i].label() = indices[i];

  if (m_index_color == 1 || m_index_color == 2)
    change_color (m_index_color);
}

bool Cluster_classification::run (int method, int classifier,
                                  std::size_t subdivisions,
                                  double smoothing)
{
  if (m_features.size() == 0)
  {
    std::cerr << "Error: features not computed" << std::endl;
    return false;
  }
  reset_indices();

  if (classifier == CGAL_CLASSIFICATION_SOWF_NUMBER)
    run (method, *m_sowf, subdivisions, smoothing);
  else if (classifier == CGAL_CLASSIFICATION_ETHZ_NUMBER)
  {
    if (m_ethz == nullptr)
    {
      std::cerr << "Error: ETHZ Random Forest must be trained or have a configuration loaded first" << std::endl;
      return false;
    }
    run (method, *m_ethz, subdivisions, smoothing);
  }
  else if (classifier == CGAL_CLASSIFICATION_OPENCV_NUMBER)
  {
#ifdef CGAL_LINKED_WITH_OPENCV
    if (m_random_forest == nullptr)
    {
      std::cerr << "Error: OpenCV Random Forest must be trained or have a configuration loaded first" << std::endl;
      return false;
    }
    run (method, *m_random_forest, subdivisions, smoothing);
#endif
  }

  return true;
}


/**
 * @example ocean/test/profile_test.cc
 */
#include <boost/test/unit_test.hpp>
#include <usml/netcdf/netcdf_files.h>
#include <usml/ocean/ocean.h>
#include <fstream>

BOOST_AUTO_TEST_SUITE(profile_test)

using namespace boost::unit_test;
using namespace usml::netcdf;
using namespace usml::ocean;

/**
 * @ingroup ocean_test
 * @{
 */

/**
 * Test the basic features of the profile model using
 * a constant profile model.
 * Generate errors if values differ by more that 1E-6 percent.
 */
BOOST_AUTO_TEST_CASE( constant_profile_test ) {
    cout << "=== profile_test: constant_profile_test ===" << endl;
    try {

        // simple values for points and speed

        wposition points(1, 1);
        matrix<double> speed(1, 1);
        wvector gradient(1, 1);

        // compute profile

        profile_linear model;
        model.sound_speed(points, &speed, &gradient);
        cout << "speed:          " << speed << endl;
        cout << "gradient rho:   " << gradient.rho() << endl;
        cout << "gradient theta: " << gradient.theta() << endl;
        cout << "gradient phi:   " << gradient.phi() << endl;

        // check the answer

        BOOST_CHECK_CLOSE(speed(0, 0), 1500.0, 1e-6);
        BOOST_CHECK_CLOSE(gradient.rho(0, 0), 0.0, 1e-6);
        BOOST_CHECK_CLOSE(gradient.theta(0, 0), 0.0, 1e-6);
        BOOST_CHECK_CLOSE(gradient.phi(0, 0), 0.0, 1e-6);
    } catch (std::exception* except) {
        BOOST_ERROR(except->what());
    }
}

/**
 * Write results of analytic test models out to CSV file for later plotting.
 * The plotted models include:
 *
 *      - Constant Profile
 *      - Linear Profile
 *      - Bi-Linear Profile
 *      - Munk Profile
 *      - N^2 Linear Profile
 *      - Catenary Profile
 */
BOOST_AUTO_TEST_CASE( plot_profile_test ) {
    cout << "=== profile_test: plot_profile_test ===" << endl;
    try {
        const char* name = USML_TEST_DIR "/ocean/test/profile_test.csv" ;
        std::ofstream os(name);
        cout << "writing tables to " << name << endl;

        // simple values for points and speed

        seq_linear depth(0.0, 200.0, 6000.0);
        const size_t D = depth.size();
        wposition points(1, D);
        for (int d = 0; d < D; ++d) {
            points.altitude(0, d, -depth(d));
        }

        // compute all analytic profiles

        matrix<double> sConstant(1, D);
        wvector gConstant(1, D);
        profile_linear constant;
        constant.sound_speed(points, &sConstant, &gConstant);
        constant.flat_earth(true);

        matrix<double> sLinear(1, D);
        wvector gLinear(1, D);
        profile_linear linear(1500.0, 0.01);
        linear.sound_speed(points, &sLinear, &gLinear);

        matrix<double> sBiLinear(1, D);
        wvector gBiLinear(1, D);
        profile_linear bilinear(1500.0, -0.02, 1300, 0.01);
        bilinear.sound_speed(points, &sBiLinear, &gBiLinear);

        matrix<double> sMunk(1, D);
        wvector gMunk(1, D);
        profile_munk munk;
        munk.sound_speed(points, &sMunk, &gMunk);

        matrix<double> sN2(1, D);
        wvector gN2(1, D);
        profile_n2 n2(1550.0, 2.4 / 1500.0);
        n2.sound_speed(points, &sN2, &gN2);

        matrix<double> sCatenary(1, D);
        wvector gCatenary(1, D);
        profile_catenary catenary(1500.0, 1e4, 1300.0);
        catenary.sound_speed(points, &sCatenary, &gCatenary);

        // save all data to CSV file

        os << "Depth" << ",FlatEarth,Gradient" << ",Linear,Gradient"
        << ",BiLinear,Gradient" << ",Munk,Gradient" << ",N2,Gradient"
        << ",Catenary,Gradient" << endl;

        for (int d = 0; d < D; ++d) {
            os << -depth(d) << "," << sConstant(0, d) << "," << gConstant.rho(
                    0, d) << "," << sLinear(0, d) << "," << gLinear.rho(0, d)
            << "," << sBiLinear(0, d) << "," << gBiLinear.rho(0, d)
            << "," << sMunk(0, d) << "," << gMunk.rho(0, d) << ","
            << sN2(0, d) << "," << gN2.rho(0, d) << "," << sCatenary(0,
                    d) << "," << gCatenary.rho(0, d) << endl;
        }
    } catch (std::exception* except) {
        BOOST_ERROR(except->what());
    }
}

/**
 * Extract Hawaii ocean temperature and salinity from World Ocean Atlas 2005.
 * Compare some of the results to the interactive version at
 * UK National Physical Laboratory, "Technical Guides -
 * Speed of Sound in Sea-Water," interactive website at
 * http://resource.npl.co.uk/acoustics/techguides/soundseawater/.
 *
 * Generate errors if values differ by more that 1E-3 percent.
 */
BOOST_AUTO_TEST_CASE( compute_mackenzie_test ) {
    cout << "=== profile_test: compute_mackenzie_test ===" << endl;
    try {
        int month = 6;
        wposition::compute_earth_radius((18.5 + 22.5) / 2.0);

        // load temperature & salinity data from World Ocean Atlas
        netcdf_woa temp(
	    USML_DATA_DIR "/woa09/temperature_seasonal_1deg.nc",
   	    USML_DATA_DIR "/woa09/temperature_monthly_1deg.nc",
            month, 18.5, 22.5, 200.5, 205.5 ) ;
        netcdf_woa sal(
	    USML_DATA_DIR "/woa09/salinity_seasonal_1deg.nc",
	    USML_DATA_DIR "/woa09/salinity_monthly_1deg.nc",
            month, 18.5, 22.5, 200.5, 205.5 ) ;
        netcdf_woa* temperature = new netcdf_woa(
	    USML_DATA_DIR "/woa09/temperature_seasonal_1deg.nc",
   	    USML_DATA_DIR "/woa09/temperature_monthly_1deg.nc",
            month, 18.5, 22.5, 200.5, 205.5 ) ;
        netcdf_woa* salinity = new netcdf_woa(
	    USML_DATA_DIR "/woa09/salinity_seasonal_1deg.nc",
	    USML_DATA_DIR "/woa09/salinity_monthly_1deg.nc",
            month, 18.5, 22.5, 200.5, 205.5 ) ;

        // compute sound speed

        profile_grid<double,3> profile(data_grid_mackenzie::construct(temperature, salinity)) ;

        // print results for first lat/long entry

        const char* name = USML_TEST_DIR "/ocean/test/mackenzie_test.csv" ;
        std::ofstream os(name);
        os.precision(12) ;
        cout << "writing tables to " << name << endl;

        size_t index[3];
        index[1] = 0;
        index[2] = 0;

        matrix<double> speed(1, 1);
        wposition location(1, 1);
        location.latitude(0, 0, 18.5);
        location.longitude(0, 0, 200.5);
        wvector gradient(1, 1);

        os << "Depth,Temp,Sal,Speed,Gradient" << endl;

        for (size_t d = 0; d < temp.axis(0)->size(); ++d) {
            index[0] = d;
            location.rho(0, 0, (*temp.axis(0))(d));
            profile.sound_speed(location, &speed, &gradient);
            os << -location.altitude(0, 0) << "," << temp.data(index)
            << "," << sal.data(index) << "," << speed(0, 0) << ","
            << -gradient.rho(0, 0) << std::endl;

            // compare to UK National Physical Laboratory software

            switch (d) {
                case 0:     // depth=0 temp=25.8543 sal=34.6954
                BOOST_CHECK_CLOSE( speed(0, 0), 1535.9781, 1e-3 );
                break;
                case 18:    // depth=1000 temp=4.3149 sal=34.5221
                BOOST_CHECK_CLOSE( speed(0, 0), 1483.6464, 1e-3 );
                break;
                case 32:    // depth=5500 temp=1.4156 sal=34.6951
                BOOST_CHECK_CLOSE( speed(0, 0), 1549.4983, 1e-3 );
                break;
                default:
                break;
            };
        }
    } catch (std::exception* except) {
        BOOST_ERROR(except->what());
    }
}

/// @}

BOOST_AUTO_TEST_SUITE_END()


#ifndef RADIUMENGINE_RAY_CAST_HPP_
#define RADIUMENGINE_RAY_CAST_HPP_

#include <Core/RaCore.hpp>
#include <Core/Types.hpp>
#include <Eigen/Core>
#include <vector>

// useful : http://www.realtimerendering.com/intersections.html

namespace Ra {
namespace Core {
// Low-level intersection functions of line versus various abstract shapes.
// All functions return true if there was a hit, false if not.
// If a ray starts inside the shape, the resulting hit will be at the ray's origin (t=0).

namespace Geometry {
class TriangleMesh;

/// Intersect a ray with an axis-aligned bounding box.
bool RA_CORE_API RayCastAabb( const Ray& r,
                              const Core::Aabb& aabb,
                              Scalar& hitOut,
                              Vector3& normalOut );

/// Intersects a ray with a sphere.
bool RA_CORE_API RayCastSphere( const Ray& r,
                                const Core::Vector3& center,
                                Scalar radius,
                                std::vector<Scalar>& hitsOut );

/// Intersect a ray with an infinite plane defined by point A and normal.
bool RA_CORE_API RayCastPlane( const Ray& r,
                               const Core::Vector3 a,
                               const Core::Vector3& normal,
                               std::vector<Scalar>& hitsOut );

/// Intersect  a ray with a cylinder with a and b as caps centers and given radius.
bool RA_CORE_API RayCastCylinder( const Ray& r,
                                  const Core::Vector3& a,
                                  const Core::Vector3& b,
                                  Scalar radius,
                                  std::vector<Scalar>& hitsOut );

/// Intersect a ray with a triangle abc.
bool RA_CORE_API RayCastTriangle( const Ray& r,
                                  const Core::Vector3 a,
                                  const Core::Vector3& b,
                                  const Core::Vector3& c,
                                  std::vector<Scalar>& hitsOut );

bool RA_CORE_API RayCastTriangleMesh( const Ray& r,
                                      const TriangleMesh& mesh,
                                      std::vector<Scalar>& hitsOut,
                                      std::vector<Vector3ui>& trianglesIdxOut );
} // namespace Geometry
} // namespace Core
} // namespace Ra

#endif // RADIUMENGINE_RAY_CAST_HPP_


//
// Created by jhwangbo on 3/12/17.
//

#ifndef RAI_RAICONTACT2_HPP
#define RAI_RAICONTACT2_HPP

#include "raisim/helper.hpp"
#include <vector>
#include <cstdlib>
#include <iostream>
#include <cmath>
#include <Eigen/Dense>
#include <raisim/Materials.hpp>
#include "raisim/math.hpp"
#include "raisim/object/Object.hpp"

namespace raisim {

class Object;

namespace contact {

class Single3DContactProblem {
 public:
  Single3DContactProblem(const MaterialPairProperties &mat, double x, double y, double z, double depth_in);
  Single3DContactProblem() = default;
  void checkRank();

  raisim::Vec<3> imp_i, tau_i, position_W;
  raisim::Mat<3, 3> MappInv_i;
  raisim::Mat<3, 3> MappInvWODel_i;
  raisim::Mat<3, 3> Mapp_i;
  raisim::Mat<2, 2> Mapp_iInv22, Mapp_i22;
  raisim::Vec<3> cooTrans_i;
  raisim::Mat<3, 2> MappInv_red;
  std::vector<raisim::Mat<3, 3>> MappInv_j;
  std::vector<Vec<3> *> imp_j;
  double mu, n2_mu, muinv, negMuSquared, coeffRes, bounceThres, bouceVel=0., Mapp_iInv11, impact_vel=0., depth=0.;
  Object *obA = nullptr, *obB = nullptr;
  int rank = 3;
  size_t pointIdA, pointIdB;
  bool atLeastOneWithoutDel = false;
};

typedef std::vector<contact::Single3DContactProblem, AlignedAllocator<contact::Single3DContactProblem, 32>>
    ContactProblems;

class BisectionContactSolver {

 public:

  struct SolverConfiguration {
    double alpha_init = 1.0;
    double alpha_low = 1.0;
    double alpha_decay = 1.0;
    double error_to_terminate = 1e-8;
    double erp = 0.3;
    double erp2 = 0.002;
    int maxIteration = 150;
  };

  explicit BisectionContactSolver(double dt) : dt_(dt), dtinv_(1/dt) {
    error_.resize(100);
  }

  void solve(ContactProblems &contact);

  void updateConfig(SolverConfiguration config) { config_=config; }

  void setTimestep(double dt) {
    dt_ = dt;
    dtinv_ = 1/dt;
  }

  void setOrder(bool order) { order_ = order; }

  int getLoopCounter() const { return loopCounter_; }

  const std::vector<double>& getErrorHistory() const { return error_; }

  SolverConfiguration& getConfig() { return config_; }
  const SolverConfiguration& getConfig() const { return config_; }

 private:
  int loopCounter_ = 0;
  bool order_ = true;
  double dt_ = 0.01;
  double dtinv_ = 100.0;
  SolverConfiguration config_;
  std::vector<double> error_;
};

} // contact
} // raisim

#endif //RAICONTACT2


/*!
 * *****************************************************************************
 *   \file faceDepthMapFrom3D_main.cpp
 *   \author moennen
 *   \brief
 *   \date 2018-03-19
 *   *****************************************************************************/

#include "utils/gl_utils.h"
#include "utils/gl_utils.inline.h"
#include "utils/imgFileLst.h"

#include "SDL2/SDL.h"
#include "SDL2/SDL_opengl.h"

// opengl
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>

// opencv
#include "utils/cv_utils.h"

// face detector / models
#include "externals/face/beFaceMModel.h"

#include <boost/filesystem.hpp>
#include <Eigen/Dense>

#include <memory>
#include <random>
#include <iostream>

#include "skinColourSamples.h"

using namespace std;
using namespace cv;
using namespace glm;
using namespace boost;

namespace
{
std::random_device rd{};
std::mt19937 rs_gen{rd()};

template <typename T>
void sampleRN( const size_t d, T* data )
{
   std::normal_distribution<T> rd;

   for ( size_t i = 0; i < d; ++i ) data[i] = rd( rs_gen );
}

vec3 sampleSkinColourFromData()
{
   static uniform_int_distribution<> rs_sample( 0, 50859 );

   const int sample = rs_sample( rs_gen );

   return vec3(
       skinColourSamples[3 * sample + 2] / 255.0,
       skinColourSamples[3 * sample + 1] / 255.0,
       skinColourSamples[3 * sample] / 255.0 );
}

vec3 sampleSkinColourFromModel()
{
   static bool isInit = false;
   static Eigen::MatrixXd skinRGB( 3, 3 );
   static Eigen::VectorXd skinColMean( 3 );
   static normal_distribution<> rs_skin_color( 0.0, 1.5 );

   // create the transform matrix from random number to skin rgb values
   if ( !isInit )
   {
      // learn from Skin_NonSkin.txt
      skinColMean << 203.992, 146.601, 113.87;
      Eigen::MatrixXd skinColCovar( 3, 3 );
      skinColCovar.row( 0 ) << 1421.03, 1296.27, 1311.81;
      skinColCovar.row( 1 ) << 1296.27, 1284.54, 1379.75;
      skinColCovar.row( 2 ) << 1311.81, 1379.75, 1731.63;

      Eigen::LLT<Eigen::MatrixXd> cholSolver( skinColCovar );
      skinRGB = cholSolver.matrixL();

      isInit = true;
   }

   Eigen::VectorXd val( 3 );
   val << rs_skin_color( rs_gen ), rs_skin_color( rs_gen ), rs_skin_color( rs_gen );

   val = skinRGB * val + skinColMean;

   return vec3( val[0] / 255.0, val[1] / 255.0, val[2] / 255.0 );
}

vec3 sampleSkinColourOffset( const vector<vec3>& vtxCol )
{
   static uniform_real_distribution<float> rs_method( 0, 1.0 );
   static uniform_real_distribution<float> rs_gain( 0.1, 1.0 );

   const float methodRand = rs_method( rs_gen );

   vec3 skinColour( 1.0 );

   if ( methodRand < 0.2 ) return skinColour;

   skinColour = ( methodRand < 0.3 ) ? sampleSkinColourFromModel() : sampleSkinColourFromData();

   vec3 vtxColour( 0.0 );
   for ( auto col : vtxCol )
   {
      vtxColour += col;
   }
   vtxColour /= vtxCol.size();

   return rs_gain( rs_gen ) * skinColour / vtxColour;
}

void fittSz( Mat& img, const uvec2 sampleSz, const float scale, const float tx, const float ty )
{
   constexpr float maxDsScaleFactor = 3.5;

   uvec2 imgSz( img.cols, img.rows );
   // random rescale
   const float minDs = std::max( (float)sampleSz.y / imgSz.y, (float)sampleSz.x / imgSz.x );
   const float ds = mix( std::min( 1.0f, maxDsScaleFactor * minDs ), minDs, scale );
   resize( img, img, Size(), ds, ds, CV_INTER_AREA );
   imgSz = ivec2( img.cols, img.rows );

   // random translate
   const ivec2 trans(
       std::floor( tx * ( imgSz.x - sampleSz.x ) ), std::floor( ty * ( imgSz.y - sampleSz.y ) ) );

   // crop
   img = img( Rect( trans.x, trans.y, sampleSz.x, sampleSz.y ) ).clone();
}

void init()
{
   glClearColor( 0.0, 0.0, 0.0, 0.0 );
   glMatrixMode( GL_PROJECTION );
   glLoadIdentity();
   // glOrtho( 0.0, windowSz.x, windowSz.y, 0.0, -10000.0, 10000.0 );
   // glEnable( GL_BLEND );
   glEnable( GL_DEPTH_TEST );
   // glFrontFace( GL_CW );
   glEnable( GL_TEXTURE_2D );
   glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
}

void drawFaceModel(
    const gl_utils::TriMeshBuffer& mdFace,
    const vec3& faceColOffset,
    const mat4& p,
    const mat4& mv,
    const vec3& lightPos,
    const vec3& lightCol,
    const float ambient )
{
   static gl_utils::RenderProgram faceShader;

   static GLint uniMVP = -1;
   static GLint uniMV = -1;
   static GLint uniMVN = -1;
   static GLint uniPosFaceCenter = -1;
   static GLint uniLightPos = -1;
   static GLint uniLightCol = -1;
   static GLint uniAmbient = -1;
   static GLint uniColOffset = -1;
   static GLint uniRoughness = -1;
   static GLint uniSubsurface = -1;
   static GLint uniSheen = -1;

   static normal_distribution<> rs_skin_roughness( 0.0, 0.1 );
   static normal_distribution<> rs_skin_subsurface( 0.0, 0.1 );
   static normal_distribution<> rs_skin_sheen( 0.5, 0.1 );

   if ( faceShader._id == -1 )
   {
      faceShader.load(
          "/mnt/p4/avila/moennen_wkspce/sceneIllEst/faceDepthMapFrom3DMM/shaders/face_frag.glsl",
          "/mnt/p4/avila/moennen_wkspce/sceneIllEst/faceDepthMapFrom3DMM/shaders/face_vtx.glsl" );
      uniMVP = faceShader.getUniform( "mvp" );
      uniMV = faceShader.getUniform( "mv" );
      uniMVN = faceShader.getUniform( "mvn" );
      uniPosFaceCenter = faceShader.getUniform( "posFaceCenter" );
      uniLightPos = faceShader.getUniform( "lightPos" );
      uniLightCol = faceShader.getUniform( "lightColor" );
      uniAmbient = faceShader.getUniform( "ambient" );
      uniColOffset = faceShader.getUniform( "colourOffset" );
      uniRoughness = faceShader.getUniform( "roughness" );
      uniSubsurface = faceShader.getUniform( "subsurface" );
      uniSheen = faceShader.getUniform( "sheen" );
   }

   faceShader.activate();

   glUniformMatrix4fv( uniMV, 1, 0, value_ptr( mv ) );

   const mat4 mvp = p * mv;
   glUniformMatrix4fv( uniMVP, 1, 0, value_ptr( mvp ) );
   const mat4 mvt = transpose( inverse( mv ) );
   glUniformMatrix4fv( uniMVN, 1, 0, value_ptr( mvt ) );

   const vec4 v4PosFaceCenter = mvp * vec4( 0.0, 0.0, 0.0, 1.0 );
   const vec2 posFaceCenter(
       v4PosFaceCenter.x / v4PosFaceCenter.w, v4PosFaceCenter.y / v4PosFaceCenter.w );
   glUniform2fv( uniPosFaceCenter, 1, value_ptr( posFaceCenter ) );

   glUniform3fv( uniLightPos, 1, value_ptr( lightPos ) );
   glUniform3fv( uniLightCol, 1, value_ptr( lightCol ) );

   glUniform1f( uniAmbient, ambient );

   glUniform1f( uniRoughness, clamp( 1.0 - abs( rs_skin_roughness( rs_gen ) ), 0.0, 1.0 ) );
   glUniform1f( uniSubsurface, clamp( 1.0 - abs( rs_skin_subsurface( rs_gen ) ), 0.0, 1.0 ) );
   glUniform1f( uniSheen, clamp( rs_skin_sheen( rs_gen ), 0.0, 1.0 ) );

   glUniform3fv( uniColOffset, 1, value_ptr( faceColOffset ) );

   mdFace.draw();

   faceShader.deactivate();
}

void draw( GLuint tex, size_t w, size_t h )
{
   // Bind Texture
   glBindTexture( GL_TEXTURE_2D, tex );

   GLfloat Vertices[] = {(float)0,
                         (float)0,
                         0,
                         (float)0 + w,
                         (float)0,
                         0,
                         (float)0 + (float)w,
                         (float)0 + (float)h,
                         0,
                         (float)0,
                         (float)0 + (float)h,
                         0};
   GLfloat TexCoord[] = {
       0,
       0,
       1,
       0,
       1,
       1,
       0,
       1,
   };
   GLubyte indices[] = {0,
                        1,
                        2,  // first triangle (bottom left - top left - top right)
                        0,
                        2,
                        3};  // second triangle (bottom left - top right - bottom right)

   glEnableClientState( GL_VERTEX_ARRAY );
   glVertexPointer( 3, GL_FLOAT, 0, Vertices );

   glEnableClientState( GL_TEXTURE_COORD_ARRAY );
   glTexCoordPointer( 2, GL_FLOAT, 0, TexCoord );

   glDrawElements( GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, indices );

   glDisableClientState( GL_TEXTURE_COORD_ARRAY );
   glDisableClientState( GL_VERTEX_ARRAY );

   glBindTexture( GL_TEXTURE_2D, 0 );
}

void getOutUVS(
    const vector<vec2>& iuvs,
    const vector<uvec3>& ivecIdx,
    const glm::uvec3* ovecIdx,
    glm::vec2* ouvs )
{
   for ( size_t f = 0; f < ivecIdx.size(); ++f )
   {
      ouvs[ovecIdx[f].x] = iuvs[ivecIdx[f].x];
      ouvs[ovecIdx[f].y] = iuvs[ivecIdx[f].y];
      ouvs[ovecIdx[f].z] = iuvs[ivecIdx[f].z];
   }
}
}
const string keys =
    "{help h usage ? |         | print this message   }"
    "{@faceModel     |         | face detection model }"
    "{@backImgLst    |         | background image list}"
    "{@nRenders      |         | number of renders    }"
    "{@startIdx      |         | idx of the first render }"
    "{@outDir        |         | output directories   }"
    "{show           |         |    }"
    "{nowrite        |        |    }";

int main( int argc, char* argv[] )
{
   CommandLineParser parser( argc, argv, keys );
   if ( parser.has( "help" ) )
   {
      parser.printMessage();
      return ( 0 );
   }

   const bool doShow = parser.get<bool>( "show" );
   const bool doWrite = !parser.get<bool>( "nowrite" );

   // Load background images
   ImgNFileLst imgLst( 1, parser.get<string>( "@backImgLst" ).c_str(), "" );
   if ( imgLst.size() == 0 )
   {
      cerr << "Invalid background image list : " << parser.get<string>( "@backImgLst" ) << endl;
      return -1;
   }
   uniform_int_distribution<> rs_img( 0, imgLst.size() - 1 );

   //------------------------------------------------------------------------- Load model
   //
   string BEfaceModelBin = parser.get<string>( "@faceModel" );

   BEFaceMModel beFaceMMd( BEfaceModelBin.c_str() );
   const unsigned nFaceParamsCoeffs = 2 * BEFaceMModel::NumCoeffs + BEFaceMModel::NumExpCoeffs;
   Eigen::VectorXf beFaceParams = Eigen::VectorXf::Zero( nFaceParamsCoeffs );

   if ( !beFaceMMd.initialized() )
   {
      vector<uvec3> vecIdx;
      vector<vec3> vecVtx;
      vector<vec2> vecUvs;
      vector<vec3> vecNormals;
      vector<vec4> vecColors;
      std::cerr << "Cannot import model -> no uvs ? " << BEfaceModelBin << std::endl;
      if ( gl_utils::loadTriangleMesh(
               "/tmp/beFaceMd.fbx", vecIdx, vecVtx, vecUvs, vecNormals, vecColors ) &&
           ( vecIdx.size() >= BEFaceMModel::NumFaces ) )
      {
         vector<vec2> ovecUvs( BEFaceMModel::NumVertices );
         getOutUVS( vecUvs, vecIdx, beFaceMMd.getFaces(), &ovecUvs[0] );
         beFaceMMd = BEFaceMModel( BEfaceModelBin.c_str(), &ovecUvs[0] );
      }
      if ( !beFaceMMd.initialized() )
      {
         std::cerr << "Cannot import model : " << BEfaceModelBin << std::endl;
         return -1;
      }
      if ( !beFaceMMd.save( "/tmp/beFaceMd.bin" ) )
      {
         std::cerr << "Cannot save model." << std::endl;
         return -1;
      }
   }

   const uvec2 imgSz( 320, 320 );

   // SDL init
   SDL_Init( SDL_INIT_EVERYTHING );
   SDL_Window* window = SDL_CreateWindow(
       "renderFaceMaps",
       SDL_WINDOWPOS_CENTERED,
       SDL_WINDOWPOS_CENTERED,
       imgSz.x,
       imgSz.y,
       SDL_WINDOW_OPENGL );
   SDL_GLContext glCtx = SDL_GL_CreateContext( window );

   // GLEW init
   GLenum err = glewInit();
   if ( GLEW_OK != err )
   {
      cerr << "Error: " << glewGetErrorString( err ) << endl;
      return -1;
   }

   // Common data alloc
   vector<vec3> vecVtx( BEFaceMModel::NumVertices, vec3( 0.0f ) );
   vector<vec3> vtxCol( BEFaceMModel::NumVertices, vec3( 0.0f ) );
   vector<vec3> vtxNorm( BEFaceMModel::NumVertices, vec3( 0.0f ) );

   // Sampler
   normal_distribution<> rs_fov( 65.0, 11.5 );

   normal_distribution<> rs_yaw( 0.0, 21.0 );
   normal_distribution<> rs_pitch( 0.0, 10.5 );
   normal_distribution<> rs_roll( 0.0, 7.0 );

   uniform_real_distribution<> rs_pos_xy( 0.25, 0.75 );
   uniform_real_distribution<> rs_pos_z( -800.0, -550.0 );
   normal_distribution<> rs_scale_off( 0.0, 1.325 );

   uniform_real_distribution<> rs_shade( 0.05, 1.0 );
   uniform_real_distribution<> rs_lightPos( -500.0, 500.0 );
   uniform_real_distribution<> rs_lightCol( 0.5, 15.0 );
   uniform_real_distribution<> rs_ambient( 0.5, 7.0 );

   uniform_real_distribution<> rs_backImgResize( 0.0, 1.0 );

   // Out root path
   const filesystem::path outRootPath( parser.get<string>( "@outDir" ) );

   const int nRenders = parser.get<int>( "@nRenders" );
   const int startIdx = parser.get<int>( "@startIdx" );
   normal_distribution<> rs_nfaces( 0.0, 0.915 );

   const int nMaxRendersPerGroup = 10000;
   unsigned renderGroupId = startIdx / nMaxRendersPerGroup;

   // Set the render buffers
   gl_utils::RenderTarget renderTarget( imgSz );
   gl_utils::Texture<gl_utils::RGBA_32FP> faceColorTex( imgSz );
   gl_utils::Texture<gl_utils::RGBA_32FP> faceUVDepthTex( imgSz );
   gl_utils::Texture<gl_utils::RGBA_32FP> faceNormalsTex( imgSz );
   gl_utils::Texture<gl_utils::RGBA_32FP> faceIdMatteTex( imgSz );
   GLuint rTex[4] = {faceColorTex.id, faceUVDepthTex.id, faceNormalsTex.id, faceIdMatteTex.id};

   for ( int s = 0; s < nRenders; ++s )
   {
      if ( ( ( startIdx + s ) % nMaxRendersPerGroup ) == 0 )
      {
         renderGroupId += 1;
         char dirname[7];
         sprintf( dirname, "%06d", renderGroupId );
         const filesystem::path outGroupRootPath =
             outRootPath / filesystem::path( std::string( dirname ) );
         if ( !filesystem::create_directory( outGroupRootPath ) )
         {
            cerr << "Cannot create directory : " << outGroupRootPath.string() << endl;
         }
      }

      // Sample a random background images
      const size_t si = rs_img( rs_gen );
      Mat backImg = cv_utils::imread32FC4( imgLst.filePath( si, 0 ) );
      if ( ( backImg.cols < imgSz.x ) || ( backImg.rows < imgSz.y ) )
      {
         --s;
         continue;
      }

      fittSz(
          backImg,
          imgSz,
          rs_backImgResize( rs_gen ),
          rs_backImgResize( rs_gen ),
          rs_backImgResize( rs_gen ) );

      renderTarget.bind( 4, &rTex[0] );
      glClear( GL_COLOR_BUFFER_BIT );
      glClear( GL_DEPTH_BUFFER_BIT );
      renderTarget.unbind();

      // Load background
      gl_utils::uploadToTexture( faceColorTex, backImg.ptr() );

      // Sample the prespective view
      const float fov = clamp( rs_fov( rs_gen ), 15.0, 110.0 );
      const mat4 camProj = glm::perspectiveFov(
          (float)( fov * M_PI / 180.0 ), (float)imgSz.x, (float)imgSz.y, 0.0001f, 10000.0f );
      const vec4 camProjectionInfo = vec4(
          -2.0 / ( imgSz.x * camProj[0][0] ),
          -2.0 / ( imgSz.y * camProj[1][1] ),
          ( 1.0 - camProj[0][2] ) / camProj[0][0],
          ( 1.0 + camProj[1][2] ) / camProj[1][1] );

      // Sample the light
      const float shade = rs_shade( rs_gen );
      const float ambient = rs_ambient( rs_gen ) * ( 1.0 - shade );
      const vec3 lightPos =
          vec3( rs_lightPos( rs_gen ), rs_lightPos( rs_gen ), rs_lightPos( rs_gen ) );
      const vec3 lightCol = vec3( rs_lightCol( rs_gen ) ) * shade;

      // Sample a random number of faces
      const size_t nfaces = 1u + std::max( -1, static_cast<int>( rs_nfaces( rs_gen ) ) );

      const float sf = std::min( 1.0 + abs( rs_scale_off( rs_gen ) ), 3.5 );
      const float gz = rs_pos_z( rs_gen );

      for ( size_t f = 0; f < nfaces; ++f )
      {
         // Sample a random face
         sampleRN( nFaceParamsCoeffs, beFaceParams.data() );

         // Create the face geometry
         beFaceMMd.get(
             beFaceParams.data(),
             beFaceParams.data() + BEFaceMModel::NumCoeffs,
             beFaceParams.data() + BEFaceMModel::NumCoeffs + BEFaceMModel::NumExpCoeffs,
             glm::value_ptr( vecVtx[0] ),
             glm::value_ptr( vtxCol[0] ) );

         gl_utils::TriMeshBuffer mdFaceA;

         // vertices
         mdFaceA.load(
             vecVtx.size(),
             vecVtx.empty() ? nullptr : &vecVtx[0],
             BEFaceMModel::NumFaces,
             beFaceMMd.getFaces() );

         // color
         mdFaceA.loadAttrib( 3, value_ptr( vtxCol[0] ) );

         // sample a random skin colour offset
         const vec3 faceColOffset = sampleSkinColourOffset( vtxCol );

         // normals
         gl_utils::computeNormals(
             BEFaceMModel::NumFaces,
             beFaceMMd.getFaces(),
             BEFaceMModel::NumVertices,
             &vecVtx[0],
             &vtxNorm[0] );
         mdFaceA.loadAttrib( 3, value_ptr( vtxNorm[0] ) );

         // uvs
         mdFaceA.loadAttrib( 2, value_ptr( beFaceMMd.getUVs()[0] ) );

         // Sample the model view
         mat4 modelView;

         const float z = gz + 200.0 * f;
         const vec2 ss_pos = vec2( rs_pos_xy( rs_gen ) * imgSz.x, rs_pos_xy( rs_gen ) * imgSz.y );
         const vec3 cs_pos = vec3(
             ( ss_pos * vec2( camProjectionInfo.x, camProjectionInfo.y ) +
               vec2( camProjectionInfo.z, camProjectionInfo.w ) ) *
                 z,
             z );
         modelView = translate( modelView, cs_pos );
         const float pitch = clamp( rs_pitch( rs_gen ), -45.0, 45.0 );
         modelView = rotate( modelView, (float)( pitch * M_PI / 180.0 ), vec3( 1.0, 0.0, 0.0 ) );
         const float yaw = clamp( rs_yaw( rs_gen ), -90.0, 90.0 );
         modelView = rotate( modelView, (float)( yaw * M_PI / 180.0 ), vec3( 0.0, 1.0, 0.0 ) );
         const float roll = clamp( rs_roll( rs_gen ), -45.0, 45.0 );
         modelView =
             rotate( modelView, (float)( M_PI + roll * M_PI / 180.0 ), vec3( 0.0, 0.0, 1.0 ) );
         modelView = glm::scale( modelView, vec3( sf ) );

         // Draw the face
         renderTarget.bind( 4, &rTex[0] );

         glEnable( GL_DEPTH_TEST );
         glEnable( GL_CULL_FACE );
         glCullFace( GL_BACK );

         drawFaceModel( mdFaceA, faceColOffset, camProj, modelView, lightPos, lightCol, ambient );

         renderTarget.unbind();
      }

      // upload and write the maps
      char sampleId[16];
      sprintf( sampleId, "%06d/%08d_", renderGroupId, startIdx + s );
      const string outBasename( sampleId );
      const string outBasenameFull = ( outRootPath / filesystem::path( sampleId ) ).string();

      Mat faceColorImg( faceColorTex.sz.y, faceColorTex.sz.x, CV_32FC4 );
      Mat faceUVDepthImg( faceUVDepthTex.sz.y, faceUVDepthTex.sz.x, CV_32FC4 );
      Mat faceNormalsImg( faceNormalsTex.sz.y, faceNormalsTex.sz.x, CV_32FC4 );
      Mat faceIdMatteImg( faceIdMatteTex.sz.y, faceIdMatteTex.sz.x, CV_32FC4 );
      if ( gl_utils::readbackTexture( faceColorTex, faceColorImg.data ) &&
           gl_utils::readbackTexture( faceUVDepthTex, faceUVDepthImg.data ) &&
           gl_utils::readbackTexture( faceNormalsTex, faceNormalsImg.data ) &&
           gl_utils::readbackTexture( faceIdMatteTex, faceIdMatteImg.data ) )
      {
         cvtColor( faceColorImg, faceColorImg, cv::COLOR_RGBA2BGR );
         cvtColor( faceUVDepthImg, faceUVDepthImg, cv::COLOR_RGBA2BGR );
         cvtColor( faceNormalsImg, faceNormalsImg, cv::COLOR_RGBA2BGR );
         cvtColor( faceIdMatteImg, faceIdMatteImg, cv::COLOR_RGBA2BGR );

         if ( doWrite )
         {
            imwrite( outBasenameFull + "c.png", faceColorImg * 255.0 );
            imwrite( outBasenameFull + "uvd.exr", faceUVDepthImg );
            imwrite( outBasenameFull + "n.exr", faceNormalsImg );
            imwrite( outBasenameFull + "idm.exr", faceIdMatteImg );
         }

         std::cout << outBasename + "c.png " << outBasename + "uvd.exr " << outBasename + "n.exr "
                   << outBasename + "idm.exr" << std::endl;

         if ( doShow )
         {
            imshow( "color", faceColorImg );
            imshow( "uvdepth", faceUVDepthImg );
            imshow( "normals", faceNormalsImg );
            imshow( "idmatte", faceIdMatteImg );
            waitKey( 0 );
         }
      }
   }

   SDL_Quit();

   return ( 0 );
}


#ifndef THYME_ENVIRONMENTS_ROVERS_ROVER
#define THYME_ENVIRONMENTS_ROVERS_ROVER

#include <Eigen/Dense>
#include <iostream>
#include <rovers/core/detail/agent_types.hpp>
#include <rovers/core/detail/entity_types.hpp>
#include <rovers/core/detail/pack.hpp>
#include <rovers/core/rewards/global.hpp>
#include <rovers/utilities/math/cartesian.hpp>
#include <vector>

namespace rovers {

/*
 *
 * rover interface
 *
 */
class IRover {
    using Point = thyme::math::Point;
    using ActionType = Eigen::MatrixXd;
    using StateType = Eigen::MatrixXd;

   public:
    IRover(double obs_radius = 1.0) : m_obs_radius(obs_radius) {}
    virtual ~IRover() = default;

    void reset() { m_path.clear(); }

    const Point& position() const { return m_position; }
    void set_position(double x, double y) {
        m_position.x = x;
        m_position.y = y;
        m_path.push_back(Point(x, y));
    }
    void update_position (double dx, double dy){
        set_position(m_position.x + dx, m_position.y + dy);
    }

    const double& obs_radius() const { return m_obs_radius; }

    const std::vector<Point>& path() const { return m_path; }

    void update() {
        // housekeeping.
        // delegate to tick for implementers.
        tick();
    }

    [[nodiscard]] virtual StateType scan(const AgentPack&) const = 0;
    [[nodiscard]] virtual double reward(const AgentPack&) const = 0;

    virtual void act(const ActionType&) = 0;

   protected:
    virtual void tick() {}

   private:
    double m_obs_radius;
    Point m_position;
    std::vector<Point> m_path;
};

/*
 *
 * Default boilerplate rover
 *
 */
template <typename SensorType, typename ActionSpace, typename RewardType = rewards::Global>
class Rover final : public IRover {
    using SType = thyme::utilities::SharedWrap<SensorType>;
    using RType = thyme::utilities::SharedWrap<RewardType>;
    using ActionType = Eigen::MatrixXd;
   public:
    Rover(double obs_radius = 1.0, SType sensor = SensorType(), RType reward = RewardType())
        : IRover(obs_radius), m_sensor(sensor), m_reward(reward) {}

    [[nodiscard]] virtual Eigen::MatrixXd scan(const AgentPack& pack) const override {
        return m_sensor->scan(pack);
    }
    [[nodiscard]] virtual double reward(const AgentPack& pack) const override {
        return m_reward->compute(pack);
    }
    void act(const ActionType& action) override {
        // default, move in x and y
        assert(action.rows() >= 2);
        auto act = static_cast<Eigen::Vector2d>(action);
        update_position(act[0], act[1]);
    }

   private:
    SType m_sensor;
    RType m_reward;
};

/*
 *
 * Example of bringing in a new Rover from the python bindings
 *
 */
class Drone final : public IRover {
   public:
    Drone(double obs_radius = 1.0) : IRover(obs_radius) {}

    [[nodiscard]] virtual Eigen::MatrixXd scan(const AgentPack&) const override { return {}; }
    [[nodiscard]] virtual double reward(const AgentPack&) const override { return 0; }
    void act(const Eigen::MatrixXd&) override { }
};

}  // namespace rovers

#endif


#include "ferns.h"
#include "seminaivebayeshyperplane.h"
#include "seminaivebayesfeature.h"
#include <boost/foreach.hpp>

Ferns::Ferns(const HyperParameters &hp) : m_hp( hp )
{
}

Ferns::~Ferns()
{
}

void Ferns::train(const matrix<float>& data, const std::vector<int>& labels, bool use_gpu)
{
    // Initialize
    initialize(data.size1());

    if (m_hp.useGPU || use_gpu)
    {
        std::vector<double> weights(labels.size(),1);
        //trainByGPU(data,labels, weights);
        std::cout << "ERROR: Not implemented yet! Jakob, do it!" << std::endl;
    }
    else
        trainByCPU(data,labels);

}

void Ferns::initialize(const int numSamples)
{
    m_confidences.resize(numSamples, m_hp.numClasses);
    m_predictions.resize(numSamples);

    for (int n = 0; n < numSamples; n++)
    {
        for (int m = 0; m < m_hp.numClasses; m++)
        {
            m_confidences(n, m) = 0.0;
        }
    }
}

void Ferns::trainByCPU(const matrix<float>& data, const std::vector<int>& labels)
{
    std::vector<int> outOfBagVoteCount(data.size1(), 0);
    matrix<float> outOfBagConfidences(data.size1(),m_hp.numClasses);
    for ( unsigned int i = 0; i < data.size1(); i++)
    {
        for ( int j = 0; j < m_hp.numClasses; j++)
        {
            outOfBagConfidences(i,j) = 0.0;
        }
    }

    HyperParameters tmpHP = m_hp;
    tmpHP.verbose = false;
    m_naiveBayesClassifiers.clear(); // be sure that they are empty
    for (int i = 0; i < m_hp.numTrees; i++)
    {
        SemiNaiveBayes::Ptr naiveClassifier;
        if (m_hp.useRandProj) {
            naiveClassifier = SemiNaiveBayes::Ptr(new SemiNaiveBayesHyperplane(tmpHP));
        }
        else {
            naiveClassifier = SemiNaiveBayes::Ptr(new SemiNaiveBayesFeature(tmpHP));
        }
        naiveClassifier->train(data,labels, m_confidences, outOfBagConfidences, outOfBagVoteCount);
        m_naiveBayesClassifiers.push_back(naiveClassifier);

        //double error = naiveClassifier->computeError(labels);
        //cout << "Error: " << error << endl;
        cout << "." << flush;
    }

    cout << endl;
    eval(data,labels);
}

void Ferns::eval(const matrix<float>& data, const std::vector<int>& labels)
{
    m_confidences.resize(data.size1(), m_hp.numClasses);
    m_predictions.resize(data.size1());
    // init to one (due to multiplication)
    for (int sample = 0; sample < (int)data.size1(); sample++)
    {
        for ( int c = 0; c < m_hp.numClasses; c++)
        {
            m_confidences(sample,c) = 0.0;
        }
    }

    BOOST_FOREACH(SemiNaiveBayes::Ptr nbc, m_naiveBayesClassifiers)
    {
        nbc->eval(data, m_confidences);
    }

    for (int sample = 0; sample < (int)m_confidences.size1();sample++)
    {
        double maxClass = 0.0;
        for ( int c = 0; c < m_hp.numClasses; c++)
        {
            if (m_confidences(sample,c) > maxClass)
            {
                m_predictions[sample] = c;
                maxClass = m_confidences(sample,c);
            }
        }
    }

    double error = computeError(labels);
    cout << "Overall Error: " << error << endl;

}

double Ferns::computeError(const std::vector<int>& labels)
{
    int error = 0;
    for (int sample = 0; sample < (int)labels.size(); sample++)
    {
        error += (m_predictions[sample] != labels[sample]) ? 1 : 0;
    }
    return static_cast<double>(error)/static_cast<double>(labels.size());
}


/**
 * sdbf_core.c: Similarity digest calculation functions.
 * authors Vassil Roussev, Candice Quates
 */

#include <boost/thread/thread.hpp>
#include <boost/math/special_functions/round.hpp>
#include <boost/interprocess/sync/interprocess_semaphore.hpp>

#include <vector>
#include "sdbf_class.h"
#include "sdbf_defines.h"

#include <boost/filesystem.hpp>
namespace fs=boost::filesystem;

boost::mutex index_output;
/**
 * Generate ranks for a file chunk.
 */
void
sdbf::gen_chunk_ranks( uint8_t *file_buffer, const uint64_t chunk_size, uint16_t *chunk_ranks, uint16_t carryover) {
    int64_t offset, entropy=0;
    uint8_t *ascii = (uint8_t *)alloc_check( ALLOC_ZERO, 256, "gen_chunk_ranks", "ascii", ERROR_EXIT);;

    if( carryover > 0) {
        memcpy( chunk_ranks, chunk_ranks+chunk_size-carryover, carryover*sizeof(uint16_t));
    }
    memset( chunk_ranks+carryover,0, (chunk_size-carryover)*sizeof( uint16_t));
    //for( offset=0; offset<chunk_size-config->entr_win_size; offset++) {
    int64_t limit = int64_t(chunk_size)-int64_t(config->entr_win_size);
    if(limit >0) {
        for (offset=0; offset<limit; offset++) {
            // Initial/sync entropy calculation
            if ( offset % config->block_size == 0) {
                entropy = config->entr64_init_int( file_buffer+offset, ascii);
            // Incremental entropy update (much faster)
            } else {
                entropy = config->entr64_inc_int( entropy, file_buffer+offset-1, ascii);
            }
            chunk_ranks[offset] = config->ENTR64_RANKS[entropy >> ENTR_POWER];
        }
    }
    free( ascii);
}

/**
 * Generate scores for a ranks chunk.
 */
void
sdbf::gen_chunk_scores( const uint16_t *chunk_ranks, const uint64_t chunk_size, uint16_t *chunk_scores, int32_t *score_histo) {
    uint64_t i, j;
    uint32_t pop_win = config->pop_win_size;
    uint64_t min_pos = 0;
    uint16_t min_rank = chunk_ranks[min_pos];

    memset( chunk_scores, 0, chunk_size*sizeof( uint16_t));
    if (chunk_size > pop_win) {
        for( i=0; i<chunk_size-pop_win; i++) {
            // try sliding on the cheap
            if( i>0 && min_rank>0) {
                while( chunk_ranks[i+pop_win] >= min_rank && i<min_pos && i<chunk_size-pop_win+1) {
                    if( chunk_ranks[i+pop_win] == min_rank)
                        min_pos = i+pop_win;
                    chunk_scores[min_pos]++;
                    i++;
                }
            }
            min_pos = i;
            min_rank = chunk_ranks[min_pos];
            for( j=i+1; j<i+pop_win; j++) {
                if( chunk_ranks[j] < min_rank && chunk_ranks[j]) {
                    min_rank = chunk_ranks[j];
                    min_pos = j;
                } else if( min_pos == j-1 && chunk_ranks[j] == min_rank) {
                    min_pos = j;
                }
            }
            if( chunk_ranks[min_pos] > 0) {
                chunk_scores[min_pos]++;
            }
        }
        // Generate score histogram (for b-sdbf signatures)
        if( score_histo) {
            for( i=0; i<chunk_size-pop_win; i++)
                score_histo[chunk_scores[i]]++;
        }
    }
}
/**
 * Generate SHA1 hashes and add them to the SDBF--original stream version.
 */
void
sdbf::gen_chunk_hash( uint8_t *file_buffer, const uint64_t chunk_pos, const uint16_t *chunk_scores, const uint64_t chunk_size) {
    uint64_t i;
    uint32_t sha1_hash[5];
    uint32_t bf_count = this->bf_count;
    uint32_t last_count = this->last_count;
    uint8_t *curr_bf = this->buffer + (bf_count-1)*(this->bf_size);
    uint32_t bigfi_count = 0;
    if (chunk_size > config->pop_win_size) {
        for( i=0; i<chunk_size-config->pop_win_size; i++) {
            if( chunk_scores[i] > config->threshold) {
                // ADD to INDEX
                SHA1( file_buffer+chunk_pos+i, config->pop_win_size, (uint8_t *)sha1_hash);
                uint32_t bits_set = bf_sha1_insert( curr_bf, 0, (uint32_t *)sha1_hash);
                // Avoid potentially repetitive features
                if( !bits_set)
                    continue;
                if (this->info!=NULL)
                    if (this->info->index) {
                        bool ins=this->info->index->insert_sha1((uint32_t*)sha1_hash);
                        if (ins==false)
                            continue;
                    }
// new style big filters...
bool inserted=this->big_filters->back()->insert_sha1((uint32_t*)sha1_hash);
if (inserted==false)
    continue;
                last_count++;
                bigfi_count++;
                if( last_count == this->max_elem) {
                    curr_bf += this->bf_size;
                    bf_count++;
                    last_count = 0;
                }
                if (bigfi_count == this->big_filters->back()->max_elem) {
                    this->big_filters->push_back(new bloom_filter(BIGFILTER,5,BIGFILTER_ELEM,0.01));
                    bigfi_count=0;
                }
            }
        }
    }
    this->bf_count = bf_count;
    this->last_count = last_count;//
}

/**
 * Generate SHA1 hashes and add them to the SDBF--block-aligned version.
 */
void
sdbf::gen_block_hash( uint8_t *file_buffer, uint64_t file_size, const uint64_t block_num, const uint16_t *chunk_scores, \
             const uint64_t block_size, class sdbf *hashto, uint32_t rem, uint32_t threshold, int32_t allowed) {

    uint8_t  *bf = hashto->buffer + block_num*(hashto->bf_size);  // BF to be filled
    uint8_t  *data = file_buffer + block_num*block_size;  // Start of data
    uint32_t  i, hash_cnt=0, sha1_hash[5];
    uint32_t  max_offset = (rem > 0) ? rem : block_size;
    uint32_t hashes[193][5];
    uint32_t num_indexes= 0;
    if (hashto->info != NULL)
        if (hashto->info->setlist != NULL)
            num_indexes=hashto->info->setlist->size();
    vector<uint32_t> match (num_indexes);
    hashto->reset_indexes(&match);
    int hashindex=0;
    for( i=0; i<max_offset-config->pop_win_size && hash_cnt< config->max_elem_dd; i++) {
        if(  chunk_scores[i] > threshold ||
            (chunk_scores[i] == threshold && allowed > 0)) {
                SHA1( data+i, config->pop_win_size, (uint8_t *)sha1_hash);
                uint32_t bits_set = bf_sha1_insert( bf, 0, (uint32_t *)sha1_hash);
                // Avoid potentially repetitive features
                if( !bits_set)
                    continue;
                if (num_indexes == 0) {
                    if (hashto->info !=NULL)
                        if (hashto->info->index)
                            hashto->info->index->insert_sha1((uint32_t*)sha1_hash);
                } else {
                    if (hash_cnt % 4 ==0) {
                        hashes[hashindex][0]=sha1_hash[0]; hashes[hashindex][1]=sha1_hash[1]; hashes[hashindex][2]=sha1_hash[2]; hashes[hashindex][3]=sha1_hash[3]; hashes[hashindex][4]=sha1_hash[4];
                        bool any=hashto->check_indexes((uint32_t*)hashes[hashindex], &match);
                        if (any)
                           hashindex++;
                        if (hashindex >=192)
                           hashindex=192;  // no more than N matches per chunk
                    }
                }
                hash_cnt++;
                if( chunk_scores[i] == threshold)
                    allowed--;
        }
    }
    if (num_indexes > 0 && hashto->info->search_first==false && hashto->info->search_deep==false) {
        // set level only for plug into assistance
        hashto->print_indexes(_FP_THRESHOLD,&match,block_num);

    }
    hashto->reset_indexes(&match);
    hashto->elem_counts[block_num] = hash_cnt;
}

/**
 * Generate SDBF hash for a buffer--stream version.
 */
void
sdbf::gen_chunk_sdbf( uint8_t *file_buffer, uint64_t file_size, uint64_t chunk_size) {
    assert( chunk_size > config->pop_win_size);

    uint32_t i, k, sum;
    int32_t score_histo[66];  // Score histogram
    uint64_t buff_size = ((file_size >> 11) + 1) << 8; // Estimate sdbf size (reallocate later)
    buff_size = (buff_size < 256) ? 256 : buff_size;                // Ensure min size
    this->buffer = (uint8_t *)alloc_check( ALLOC_ZERO, buff_size, "gen_chunk_sdbf", "sdbf_buffer", ERROR_EXIT);

    // Chunk-based computation
    uint64_t qt = file_size/chunk_size;
    uint64_t rem = file_size % chunk_size;

    uint64_t chunk_pos = 0;
    uint16_t *chunk_ranks = (uint16_t *)alloc_check( ALLOC_ONLY, (chunk_size)*sizeof( uint16_t), "gen_chunk_sdbf", "chunk_ranks", ERROR_EXIT);
    uint16_t *chunk_scores = (uint16_t *)alloc_check( ALLOC_ZERO, (chunk_size)*sizeof( uint16_t), "gen_chunk_sdbf", "chunk_scores", ERROR_EXIT);

    for( i=0; i<qt; i++, chunk_pos+=chunk_size) {
        gen_chunk_ranks( file_buffer+chunk_size*i, chunk_size, chunk_ranks, 0);
        memset( score_histo, 0, sizeof( score_histo));
        gen_chunk_scores( chunk_ranks, chunk_size, chunk_scores, score_histo);

        // Calculate thresholding paremeters
        for( k=65, sum=0; k>=config->threshold; k--) {
            if( (sum <= this->max_elem) && (sum+score_histo[k] > this->max_elem))
                break;
            sum += score_histo[k];
        }
        //allowed = this->max_elem-sum;
        gen_chunk_hash( file_buffer, chunk_pos, chunk_scores, chunk_size);
    }
    if( rem > 0) {
        gen_chunk_ranks( file_buffer+qt*chunk_size, rem, chunk_ranks, 0);
        gen_chunk_scores( chunk_ranks, rem, chunk_scores, 0);
        gen_chunk_hash( file_buffer, chunk_pos, chunk_scores, rem);
    }

    // Chop off last BF if its membership is too low (eliminates some FPs)
    if( this->bf_count > 1 && this->last_count < this->max_elem/8) {
        this->bf_count = this->bf_count-1;
        this->last_count = this->max_elem;
    }
    // Trim BF allocation to size
    if( this->bf_count*this->bf_size < buff_size) {
        this->buffer = (uint8_t*)realloc_check( this->buffer, (this->bf_count*this->bf_size));
    }
    free( chunk_ranks);
    free( chunk_scores);

}

/**
 * Worker thread for multi-threaded block hash generation.  // NOT iN CLASS?
 */
void *
sdbf::thread_gen_block_sdbf( void *task_param) {
    uint32_t i, k, sum, allowed;
    int32_t  score_histo[66];
    blockhash_task_t *hashtask = (blockhash_task_t *)task_param;
    uint64_t block_size = hashtask->block_size;
    uint8_t *buffer = hashtask->buffer;
    uint64_t file_size = hashtask->file_size;

    uint64_t qt = file_size/block_size;

    uint64_t chunk_pos = 0;
    uint16_t *chunk_ranks = (uint16_t *)alloc_check( ALLOC_ONLY, (block_size)*sizeof( uint16_t), "gen_block_sdbf", "chunk_ranks", ERROR_EXIT);
    uint16_t *chunk_scores = (uint16_t *)alloc_check( ALLOC_ZERO, (block_size)*sizeof( uint16_t), "gen_block_sdbf", "chunk_scores", ERROR_EXIT);

    for( i=hashtask->tid; i<qt; i+=hashtask->tcount, chunk_pos+=hashtask->tcount*block_size) {
        gen_chunk_ranks( buffer+block_size*i, block_size, chunk_ranks, 0);
        memset( score_histo, 0, sizeof( score_histo));
        gen_chunk_scores( chunk_ranks, block_size, chunk_scores, score_histo);
        // Calculate thresholding paremeters
        for( k=65, sum=0; k>=config->threshold; k--) {
            if( (sum <= config->max_elem_dd) && (sum+score_histo[k] > config->max_elem_dd))
                break;
            sum += score_histo[k];
        }
        allowed = config->max_elem_dd-sum;
        gen_block_hash( buffer, file_size, i, chunk_scores, block_size, hashtask->sdbf, 0, k, allowed);
    }
    free( chunk_ranks);
    free( chunk_scores);
    return NULL;
}

/**
    dd-mode hash generation.
*/
void
sdbf::gen_block_sdbf_mt( uint8_t *file_buffer, uint64_t file_size, uint64_t block_size,  uint32_t thread_cnt) {

    blockhash_task_t *tasks = (blockhash_task_t *) alloc_check( ALLOC_ONLY, thread_cnt*sizeof( blockhash_task_t), "gen_block_sdbf_mt", "tasks", ERROR_EXIT);
    boost::thread *hash_pool[MAX_THREADS];
    uint32_t t;
    for( t=0; t<thread_cnt; t++) {
        tasks[t].tid = t;
        tasks[t].tcount = thread_cnt;
        tasks[t].buffer = file_buffer;
        tasks[t].file_size = file_size;
        tasks[t].block_size = block_size;
        tasks[t].sdbf = this;
        hash_pool[t] = new boost::thread(sdbf::thread_gen_block_sdbf,tasks+t);
    }
    for( t=0; t<thread_cnt; t++) {
        hash_pool[t]->join();
    }
    // Deal with the "tail" if necessary
    uint64_t qt = file_size/block_size;
    uint64_t rem = file_size % block_size;

    if( rem >= MIN_FILE_SIZE) {
        uint16_t *chunk_ranks = (uint16_t *)alloc_check( ALLOC_ONLY, (block_size)*sizeof( uint16_t), "gen_block_sdbf_mt", "chunk_ranks", ERROR_EXIT);
        uint16_t *chunk_scores = (uint16_t *)alloc_check( ALLOC_ZERO, (block_size)*sizeof( uint16_t), "gen_block_sdbf_mt", "chunk_scores", ERROR_EXIT);

        gen_chunk_ranks( file_buffer+block_size*qt, rem, chunk_ranks, 0);
        gen_chunk_scores( chunk_ranks, rem, chunk_scores, NULL);
        gen_block_hash( file_buffer, file_size, qt, chunk_scores, block_size, this, rem, config->threshold, this->max_elem);

        free( chunk_ranks);
        free( chunk_scores);
    }
    for( t=0; t<thread_cnt; t++) {
         delete hash_pool[t];
    }
    free(tasks);
}

/**
 * Calculates the score between two digests
 */
int
sdbf::sdbf_score( sdbf *sdbf_1, sdbf *sdbf_2, uint32_t sample) {

    double max_score, score_sum = -1;
    uint32_t i;
    uint32_t bf_count_1, rand_offset;
    sdbf_task_t *tasklist = NULL;

    if( !sdbf_1->hamming)
        sdbf_1->compute_hamming();
    if( !sdbf_2->hamming)
        sdbf_2->compute_hamming();

    // if sampling, set sample count here.
    if ((sample > 0 ) && (sdbf_1->bf_count > sample)) {
        bf_count_1 = sample;
    } else {
        bf_count_1 = sdbf_1->bf_count;
    }
    // Make sure |sdbf_1| <<< |sdbf_2|
    // does this matter anymore?
    if( (bf_count_1 > sdbf_2->bf_count) ||
        (bf_count_1 == sdbf_2->bf_count &&
            ((get_elem_count( sdbf_1, bf_count_1-1) > get_elem_count( sdbf_2, sdbf_2->bf_count-1)) ||
              strcmp( (char*)sdbf_1->hashname, (char*)sdbf_2->hashname) > 0 ))) {
            sdbf *tmp = sdbf_1;
            sdbf_1 = sdbf_2;
            sdbf_2 = tmp;
            bf_count_1 = sdbf_1->bf_count;
    }

    tasklist = (sdbf_task_t *) alloc_check( ALLOC_ZERO, sizeof( sdbf_task_t), "sdbf_score", "tasklist", ERROR_EXIT);
    tasklist[0].tid = 0;
    tasklist[0].tcount = 1;
    tasklist[0].ref_sdbf = sdbf_1;
    tasklist[0].tgt_sdbf = sdbf_2;
    tasklist[0].done = 0;
    rand_offset = 1 ;
    srand(time(NULL));
    int sparsect=0;
    for( i=0; i< bf_count_1; i++) {
        if (sample > 0 && bf_count_1 > sample)  {
            rand_offset = rand() % (uint32_t)(sdbf_1->bf_count / sample);
        }
        tasklist[0].ref_index=i*rand_offset;
        max_score = sdbf_max_score( tasklist);
        score_sum = (score_sum < 0) ? max_score : score_sum + max_score;
        if (get_elem_count( sdbf_1, i) < MIN_ELEM_COUNT )
            sparsect++;
    }
    uint64_t denom = bf_count_1;
    // improving the average.
    if (bf_count_1 > 1)
        denom-=sparsect;
    free(tasklist);
    if (denom == 0)
        score_sum = -1;
    return (score_sum < 0) ? -1 : boost::math::round( 100.0*score_sum/(denom));

}

/**
 * Given a BF and an SDBF, calculates the maximum match (0-100)
 */

double
sdbf::sdbf_max_score( sdbf_task_t *task) {
    assert( task != NULL);

    double score, max_score=-1;
    uint32_t i, s1, s2, max_est, match, cut_off, slack=48;
    uint32_t bf_size = task->ref_sdbf->bf_size;
    uint16_t *bf_1, *bf_2;

    s1 = get_elem_count( task->ref_sdbf, task->ref_index);
    if( s1 < MIN_ELEM_COUNT)
        return 0;
    bf_1 = (uint16_t *)(task->ref_sdbf->buffer + task->ref_index*bf_size);
    uint32_t e1_cnt = task->ref_sdbf->hamming[task->ref_index];
    uint32_t comp_cnt = task->tgt_sdbf->bf_count;
    for( i=task->tid; i<comp_cnt; i+=task->tcount) {
        bf_2 = (uint16_t *)(task->tgt_sdbf->buffer + i*bf_size);
        s2 = get_elem_count( task->tgt_sdbf, i);
        if( task->ref_sdbf->bf_count >= 1 && s2 < MIN_ELEM_COUNT)
            continue;
        uint32_t e2_cnt = task->tgt_sdbf->hamming[i];

        // Max/min number of matching bits & zero cut off
        max_est = (e1_cnt < e2_cnt) ? e1_cnt : e2_cnt;
        //min_est = bf_match_est( 8*bf_size, task->ref_sdbf->hash_count, s1, s2, 0);
        //cut_off = boost::math::round( SD_SCORE_SCALE*(double)(max_est-min_est)+(double)min_est);
        int mn;
        if (!task->ref_sdbf->fastmode) {
            mn=boost::math::round(4096/(s1+s2));
            cut_off=config->CUTOFFS256[mn];
        } else {
            mn=boost::math::round(1024/(s1+s2));
            cut_off=config->CUTOFFS64[mn];
        }
        // Find matching bits
        if (config->popcnt) {
            if (!task->ref_sdbf->fastmode)
                match = bf_bitcount_cut_256_asm( (uint8_t *)bf_1, (uint8_t *)bf_2, cut_off, slack);
            else
                match = bf_bitcount_cut_64_asm( (uint8_t *)bf_1, (uint8_t *)bf_2, cut_off, slack);
            if( match > 0) {
                if (!task->ref_sdbf->fastmode)
                    match = bf_bitcount_cut_256_asm( (uint8_t *)bf_1, (uint8_t *)bf_2, 0, 0);
                else
                    match = bf_bitcount_cut_64_asm( (uint8_t *)bf_1, (uint8_t *)bf_2, 0, 0);
            }
        } else {
            match = bf_bitcount_cut_256( (uint8_t *)bf_1, (uint8_t *)bf_2, cut_off, slack);
            if( match > 0) {
                match = bf_bitcount_cut_256( (uint8_t *)bf_1, (uint8_t *)bf_2, 0, 0);
            }

        }
        score = (match <= cut_off) ? 0 : (double)(match-cut_off)/(max_est-cut_off);
        max_score = (score > max_score) ? score : max_score;
    }

    task->result = max_score;

    return max_score;
}

void
sdbf::print_indexes(uint32_t threshold, vector<uint32_t> *matches, uint64_t pos){
    uint32_t count=this->info->setlist->size();
    std::stringstream build;
    bool any=false;
    for (uint32_t i=0;i<count;i++) {
        if (matches->at(i) > threshold) {
            build << this->name() <<" [" << pos << "] |" << this->info->setlist->at(i)->name() << "|" << matches->at(i) << endl;
            any=true;
        }
    }
    if (any) {
        boost::lock_guard<boost::mutex> lock(index_output);
        index_results.append(build.str());
    }
}

void
sdbf::reset_indexes(vector<uint32_t> *matches){
    std::fill(matches->begin(),matches->end(),0);
    if (matches->size()==1)
    matches->at(0)=0;
}

bool
sdbf::check_indexes(uint32_t* sha1, vector<uint32_t>* matches) {
    uint32_t count=this->info->setlist->size();
    bool any=false;
    for (uint32_t i=0;i<count;i++) {
        if (this->info->setlist->at(i)->index->query_sha1(sha1)) {
            matches->at(i)++;
            any=true;
        }
    }
    return any;
}


#ifndef CX_DQMC_ABSTRACT_GREENS_HPP
#define CX_DQMC_ABSTRACT_GREENS_HPP

#include "la.hpp"
#include "parameters.hpp"
#include "cx_workspace.hpp"

#include <alps/alea.h>
#include <boost/multi_array.hpp>
#include <boost/timer/timer.hpp>
#include <exception>
#include <cstring>
#include <valarray>
#include <algorithm>

typedef boost::multi_array<double, 3> aux_spin_3t;

namespace cx_dqmc {

    class abstract_greens {

    public:
	cx_mat X, Y, X_sls, Y_sls, Y_sls_row_1, Y_sls_row_2;
	int delayed_buffer_size;

	cx_mat greens, temp_greens, prop_greens;
	bool first_initialization;

	double spin, delta_tau;
	cx_double_t det_sign, phase;
	pdouble_t det, log_det, curr_log_det, diff;
	int N, n_A, n_B, particles, eff_particles;
	int vol, sites, replica;
    	int direction, current_slice, current_bond,
	    slices, n_elements, safe_mult, chunks;
	int start, stop, idx;
	int indices[2];
	bool changed_direction, new_sign, fresh_det, dir_switch;
	double lowest_sv;

	std::vector<cx_mat_t> u_stack, t_stack;
	std::vector<vec_t> d_stack;

	std::vector<cx_mat_t*> Us, Ts, large_mats;
	std::vector<vec_t*> Ds, large_vecs;

	std::vector<int> U_is_unitary, T_is_unitary;

	std::valarray<double> svds;
	std::valarray<double> svd_example;
	std::valarray<double> stability_checks;
	std::valarray<double> stability;

	abstract_greens(dqmc::parameters * p_, cx_dqmc::workspace * ws_,
			cx_dqmc::workspace * reg_ws_, aux_spin_3t * s)
	    : p(p_), ws(ws_), reg_ws(reg_ws_), aux_spins(s) {};

	// abstract_greens(dqmc::parameters * p_, cx_dqmc::workspace * ws_,
	// 			     aux_spin_3t * s)
	//     : p(p_), ws(ws_), reg_ws(ws_), aux_spins(s) {};

	cx_dqmc::workspace * ws;
	cx_dqmc::workspace * reg_ws;
	dqmc::parameters * p;
	aux_spin_3t * aux_spins;

	virtual void initialize() = 0;
	virtual void build_stack() = 0;
	virtual int propagate(alps::Observable& stability) = 0;
	virtual void log_weight() = 0;
	virtual void log_weight_full_piv() = 0;
	virtual void update_remove_interaction() = 0;
	virtual void update_add_interaction() = 0;

	int get_direction() { return direction; };
	int get_slice() { return current_slice; }
	void print_diag() { std::cout << greens.diagonal().transpose() << std::endl; }
	cx_mat& get_greens() { return greens; }
	cx_mat * get_greens_ptr() { return &greens; }
	void save_curr_det() { curr_log_det = log_det; }
	void update_det() {
	    curr_log_det = log_det;
	}

	double get_log_det() {
	    if (fresh_det == false) {
		throw std::runtime_error("det is not fresh");
	    }
	    return log_det; }

	cx_double get_det_sign() { return det_sign; }
	cx_double& at(int i, int j) { return greens(i, j); }
	cx_double get_tr() {
	    return greens.trace();
	    // throw std::runtime_error("not available yet");
	    //return {0., 0.};
	}

    };
}
#endif


#ifndef CX_DQMC_GREENS_REPLICA_FT_HPP
#define CX_DQMC_GREENS_REPLICA_FT_HPP

#include <complex>
#include <libdqmc/cx_dqmc_abstract_greens.hpp>
#include <libdqmc/la.hpp>
#include <libdqmc/cx_density.hpp>
#include <libdqmc/parameters.hpp>
#include <libdqmc/cx_workspace.hpp>
#include <libdqmc/cx_checkerboard.hpp>
#include <libdqmc/cx_interactions.hpp>
#include <libdqmc/cx_calculate_greens_ft.hpp>
#include <alps/alea.h>
#include <boost/multi_array.hpp>
#include <exception>
#include <cstring>

typedef boost::multi_array<double, 3> aux_spin_3t;

namespace cx_dqmc {

    class greens_replica_ft : public cx_dqmc::abstract_greens {

    public:
	bool first_initialization, dir_switch;

	cx_mat_t u_temp, t_temp;
	vec_t d_temp;
	cx_mat_t Ul, Um, Ur, Tl, Tm, Tr;
	vec_t Dl, Dm, Dr;

	greens_replica_ft(dqmc::parameters * p_, cx_dqmc::workspace * ws_,
			aux_spin_3t * s)
	    : cx_dqmc::abstract_greens(p_, ws_, ws_, s) {};

	void initialize();
	void build_stack();
	int propagate(alps::Observable& stability);

	void ket_update(int start, int stop,
			int idx, int target);
	void bra_update(int start, int stop,
			int idx, int target);

	void update_remove_interaction();
	void update_add_interaction();

	void slice_sequence(int start, int stop) {};
	void slice_sequence_left(int start, int stop, cx_mat& M);
	void slice_sequence_left_t(int start, int stop, cx_mat& M);
	void slice_sequence_right(int start, int stop, cx_mat& M);

	void slice_matrix_left(int slice, cx_mat& M, bool inv = false);
	void slice_matrix_left_t(int slice, cx_mat& M, bool inv = false);
	void slice_matrix_right(int slice, cx_mat& M, bool inv = false);

	void hopping_matrix_left(int slice, cx_mat& M, bool inv = false);
	void hopping_matrix_right(int slice, cx_mat& M, bool inv = false);

	void interaction_matrix_left(int slice, cx_mat& M,
				     int bond_group, bool inv = false);
	void interaction_matrix_right(int slice, cx_mat& M,
				      int bond_group, bool inv = false);

	void regularize_svd(vec& in, vec& out);

	void calculate_greens(int method = 0);
	void log_weight();
	void calculate_greens_general();
	void calculate_greens_exact(int slice);
	void calculate_greens(cx_mat U, cx_mat& G);

    	double check_stability();
    };
}
#endif