//============================================================================
//
// This file is part of the Shapes Generative Model Library (SGML) project.
//
// Copyright (c) 2010-2014 - Evangelos Kalogerakis, Siddhartha Chaudhuri (authors of the code) / Stanford University, UMass-Amherst
//
// SGML 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.

// SGML 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 SGML.  If not, see <http://www.gnu.org/licenses/>.
//
//============================================================================

#include "S12_ShapeBNet.hpp"
#include "S12_MeshCreator.hpp"

namespace SNL {
namespace S12 {

void ShapeBNet::internal_clean()
{
  if (root_node != NULL)
  {
    delete root_node;
    root_node = NULL;
  }

  for (size_t la = 0; la < layout_nodes.size(); la++)
    delete layout_nodes[la];

  for (size_t la = 0; la < cardinality_nodes.size(); la++)
    delete cardinality_nodes[la];

  for (size_t la = 0; la < adjacencynumparts_nodes.size(); la++)
    delete adjacencynumparts_nodes[la];

  for (size_t g = 0; g < gaussian_nodes.size(); g++)
    delete gaussian_nodes[g];

  if (nodes != NULL)
  {
    delete nodes;
    nodes = NULL;
  }

}


void ShapeBNet::internal_clean(SNL_VECTOR(SNL::Node *)* nodes)
{
  for ( NodeIterator i = nodes->begin(); i != nodes->end(); i++ )
  {
    if ( (*i)->type == 'R' )
      delete (RootNode*)(*i);
    if ( (*i)->type == 'L' )
      delete (LayoutNode*)(*i);
    if ( (*i)->type == 'C' )
      delete (CardinalityNode*)(*i);
    if ( (*i)->type == 'G' )
      delete (GaussianNode*)(*i);
    if ( (*i)->type == 'A' )
      delete (AdjacencyNumPartsNode*)(*i);
  }

  delete nodes;
}


ShapeBNet::~ShapeBNet()
{
  internal_clean();
}


void ShapeBNet::init(size_t root_domain_size, SNL_VECTOR( size_t ) & add_layout_domain_size, bool prepare_for_training)
{
  int num_kmeans_replicates = options.get("kmeans_replicates", 3000);
  int num_kmeans_iterations = options.get("kmeans_iterations", 100);
  if ( !prepare_for_training)
  {
    num_kmeans_replicates = 1;
    num_kmeans_iterations = 1;
  }
  double em_initialization_randomness = options.get("em_initialization_randomness", 0.0);

  // initialize root node
  SNL_VECTOR(SNL::Node *)* _nodes = new SNL_VECTOR(SNL::Node *)();
  if (root_node != NULL)
    if (root_node->istemp) // root_node frmo the constructor
      delete root_node;
  this->layout_nodes.clear();
  this->gaussian_nodes.clear();
  this->cardinality_nodes.clear();
  this->adjacencynumparts_nodes.clear();

  root_node = new RootNode();
  _nodes->push_back(root_node);
  root_node->domain_size = root_domain_size;
  root_node->values = increasingNumberVector((size_t)0, root_domain_size);
  root_node->add_layout_domain_size = add_layout_domain_size;

  // initialize other nodes
  size_t num_adj_nodes = 0;
  for (size_t l = 0; l < labels.size(); l++)
  {
    LayoutNode * cn = new LayoutNode(labels.at(l), root_node, 1+l);
    _nodes->push_back(cn);
    this->layout_nodes.push_back(cn);
    cn->domain_size = 2 + root_node->add_layout_domain_size[l];      // +1, value 0 corresponds to no such part exists
    cn->values = increasingNumberVector((size_t)0, cn->domain_size);
    cn->Pl.resize(meshes.size(), cn->domain_size, 0.0);
    cn->Plr.resize(meshes.size());

    for (size_t m = 0; m < meshes.size(); m++)
    {
      cn->Plr[m].resize(cn->domain_size, root_domain_size, 0.0);
    }

    // initialize cn->Pl
    SNL_VECTOR( SNL_VECTOR(double) ) F;
    for (size_t m = 0; m < meshes.size(); m++)
    {
      if (!features_training_data[m][l].empty())
      {
        F.push_back(concatenateVectors(features_training_data[m][l], adjacentparts_training_data[m][l]));
      }
    }
    KMeans<double> kmeans(F, root_node->add_layout_domain_size[l] + 1,  num_kmeans_replicates, num_kmeans_iterations);
    size_t k = 0;

    for (size_t m = 0; m < meshes.size(); m++)
    {
      if (!features_training_data[m][l].empty())
      {
        cn->Pl[m][ 1 + kmeans.CI[k] ] = 1.0;

        for (size_t kk = 0; kk < cn->domain_size; kk++)
        {
          double r = unifRand<double>(0.0, em_initialization_randomness);
          cn->Pl[m][ kk ] += r;
        }

        k++;
      }
      else
      {
        cn->Pl[m][ 0 ] = 1.0;
      }

      if (!features_training_data[m][l].empty())
      {
        double normalize_pc = 0.0;

        for (size_t kk = 0; kk < cn->domain_size; kk++)
          normalize_pc += cn->Pl[m][ kk ];

        for (size_t kk = 0; kk < cn->domain_size; kk++)
          cn->Pl[m][ kk ] /= normalize_pc;
      }
    }


    GaussianNode * gn = new GaussianNode(cn, 1+l+labels.size());
    _nodes->push_back(gn);
    this->gaussian_nodes.push_back(gn);

    CardinalityNode * card_node = new CardinalityNode(labels.at(l), root_node, layout_nodes[l], 1+l+2*labels.size());
    _nodes->push_back(card_node);
    for (size_t m = 0; m < meshes.size(); m++)
      card_node->registerValue(cardinality_training_data[m][l]);
    this->cardinality_nodes.push_back(card_node);

    for (size_t a = 0; a < labels.at(l)->adj_labels.size(); a++)
    {
      num_adj_nodes++;
      AdjacencyNumPartsNode * adj_part_node = new AdjacencyNumPartsNode(labels.at(l), this->labels.at(l)->adj_labels[a], a, cn, num_adj_nodes+3*labels.size());
      _nodes->push_back( adj_part_node );
      for (size_t m = 0; m < meshes.size(); m++)
        adj_part_node->registerValue(adjacentparts_training_data[m][l][a]);
      this->adjacencynumparts_nodes.push_back(adj_part_node);
    }
  }

  // initialize Pr of root node
  root_node->Pr.resize(meshes.size(), root_node->domain_size, 0.0);
  SNL_VECTOR( SNL_VECTOR(double) ) F;

  for (size_t m = 0; m < meshes.size(); m++)
  {
    SNL_VECTOR(double) Fi;

    for (size_t l = 0; l < labels.size(); l++)
    {
      for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
      {
        Fi.push_back( layout_nodes[l]->Pl[m][d2] );
      }
      for (size_t d2 = 0; d2 < cardinality_nodes[l]->domain_size; d2++)
      {
        Fi.push_back( double(cardinality_nodes[l]->getValuePos(cardinality_training_data[m][l]) == d2) );
      }
    }

    F.push_back(Fi);
  }
  KMeans<double> kmeans(F, root_node->domain_size,  num_kmeans_replicates, num_kmeans_iterations);

  for (size_t m = 0; m < meshes.size(); m++)
    root_node->Pr[m][ kmeans.CI[m] ] = 1.0;

  // add a bit of randomness
  for (size_t m = 0; m < meshes.size(); m++)
  {
    double normalize_pr = 0.0;
    for (size_t d = 0; d < root_node->domain_size; d++)
    {
      double r = unifRand<double>(0.0, em_initialization_randomness);
      root_node->Pr[m][d] += r;
      normalize_pr += root_node->Pr[m][d];
    }
    for (size_t d = 0; d < root_node->domain_size; d++)
      root_node->Pr[m][d] /= normalize_pr;
  }


  // initialize Plr
  for (size_t l = 0; l < labels.size(); l++)
    for (size_t m = 0; m < meshes.size(); m++)
      for (size_t d = 0; d < root_node->domain_size; d++)
        for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
          layout_nodes[l]->Plr[m][d2][d] = root_node->Pr[m][d] * layout_nodes[l]->Pl[m][d2];

        // initialize most_likely_initial_meshes
					style_id.resize( meshes.size() );
          root_node->most_likely_initial_meshes.clear();
          root_node->most_likely_initial_meshes.resize(root_node->domain_size);
          for (size_t m = 0; m < meshes.size(); m++)
          {
            size_t maximum_pr_m = maximumId(root_node->Pr.getRow(m));
						style_id[m] = maximum_pr_m;
            root_node->most_likely_initial_meshes[ maximum_pr_m ].push_back(meshes.at(m));
          }

          for (size_t l = 0; l < labels.size(); l++)
          {
            layout_nodes[l]->most_likely_initial_meshes.clear();
            layout_nodes[l]->most_likely_initial_meshes.resize(layout_nodes[l]->domain_size);
          }

          for (size_t m = 0; m < meshes.size(); m++)
          {
            for (size_t l = 0; l < labels.size(); l++)
            {
              size_t maximum_pr_m = maximumId(layout_nodes[l]->Pl.getRow(m));
              layout_nodes[l]->most_likely_initial_meshes[ maximum_pr_m ].push_back(meshes.at(m));
            }
          }


          // save old_Pr in case previous structure search got less score
          root_node->old_Pr = root_node->Pr;
          for (size_t l = 0; l < labels.size(); l++)
          {
            layout_nodes[l]->old_Pl = layout_nodes[l]->Pl;
            layout_nodes[l]->old_Plr = layout_nodes[l]->Plr;
          }


  this->nodes = _nodes;
}


void ShapeBNet::init( SNL_VECTOR(SNL::Node *)* _nodes )
{
  // initialize root node
  if (root_node != NULL)
    if (root_node->istemp)
    delete root_node;
  this->layout_nodes.clear();
  this->gaussian_nodes.clear();
  this->cardinality_nodes.clear();
  this->adjacencynumparts_nodes.clear();
  SNL_VECTOR(SNL::Node *)::iterator it = _nodes->begin();

  root_node = (RootNode*)*(it);
  it++;
  for (size_t l = 0; l < labels.size(); l++)
  {
    LayoutNode* cn = (LayoutNode*)*( it );
    this->layout_nodes.push_back(cn);
    it++;

    GaussianNode* gn = (GaussianNode *)*( it );
    this->gaussian_nodes.push_back(gn);
    it++;

    CardinalityNode * card_node = (CardinalityNode *)*( it );
    this->cardinality_nodes.push_back(card_node);
    it++;

    for (size_t a = 0; a < labels.at(l)->adj_labels.size(); a++)
    {
      AdjacencyNumPartsNode * adj_part_node = (AdjacencyNumPartsNode*)*( it );
      this->adjacencynumparts_nodes.push_back(adj_part_node);
      it++;
    }
  }

  this->nodes = _nodes;
}


bool ShapeBNet::topologicalSort()
{
  ordered_cardinality_nodes.clear();
  ordered_gaussian_nodes.clear();

  size_t num_tries = 0;
  SNL_VECTOR(bool) done(labels.size(), false);
  bool all_done = false;

  while (!all_done)
  {
    if (num_tries > labels.size())
      return false;

    num_tries++;

    for (size_t l = 0; l < labels.size(); l++)
    {
      if (done[l])
        continue;

      bool ready_to_be_sampled = true;

      for (size_t p = 0; p < cardinality_nodes[l]->parents_CardinalityNode.size(); p++)
      {
        size_t lx = cardinality_nodes[l]->parents_CardinalityNode[p]->label->id;
        ready_to_be_sampled &= done[ lx ];
      }

      if (!ready_to_be_sampled)
        continue;

      ordered_cardinality_nodes.push_back(cardinality_nodes[l]);
      done[l] = true;
    }

    all_done = true;

    for (size_t l = 0; l < labels.size(); l++)
      all_done &= done[l];
  }

  num_tries = 0;
  done.clear();
  done.resize(labels.size(), false);
  all_done = false;

  while (!all_done)
  {
    if (num_tries > labels.size())
      return false;

    num_tries++;

    for (size_t l = 0; l < labels.size(); l++)
    {
      if (done[l])
        continue;

      bool ready_to_be_sampled = true;

      for (size_t p = 0; p < gaussian_nodes[l]->parents_gaussians.size(); p++)
      {
        size_t lx = gaussian_nodes[l]->parents_gaussians[p]->layout_node->label->id;
        ready_to_be_sampled &= done[ lx ];
      }

      if (!ready_to_be_sampled)
        continue;

      ordered_gaussian_nodes.push_back(gaussian_nodes[l]);
      done[l] = true;
    }

    all_done = true;

    for (size_t l = 0; l < labels.size(); l++)
      all_done &= done[l];
  }

  return true;
}


void ShapeBNet::setTrainingData(std::vector<Label *> & _labels, std::vector<MeshPart *> & _parts, std::vector<Mesh *> & _meshes)
{
  bool deactivate_adjacencies = options.get("deactivate_adjacencies", false);

  this->parts = _parts;
  this->meshes = _meshes;
  this->labels = _labels;

  // find number of parts per label for each mesh
  for (size_t m = 0; m < meshes.size(); m++)
    set_number_of_parts_per_label(*meshes[m], labels);

  // define reference part
  SNL_VECTOR( SNL_VECTOR( MeshPart * ) ) ref_parts(meshes.size());

  for (size_t m = 0; m < meshes.size(); m++)
  {
    ref_parts[m].resize(labels.size(), NULL);

    for (size_t l = 0; l < labels.size(); l++)
    {
      for (size_t pm = 0; pm < meshes[m]->parts.size(); pm++)
      {
        MeshPart * part = parts[ meshes[m]->parts[pm]->id ];

        if (part->sublabel_id == 1 && part->label == labels.at(l))
        {
          ref_parts[m][l] = part;
        }
      }

      if (ref_parts[m][l] == NULL)  // choose random part - rare
      {
        for (size_t pm = 0; pm < meshes[m]->parts.size(); pm++)
        {
          MeshPart * part = parts[ meshes[m]->parts[pm]->id ];

          if (part->label == labels.at(l))
          {
            ref_parts[m][l] = part;
          }
        }
      }
    }
  }


  features_training_data.resize(meshes.size());
  adjacentparts_training_data.resize(meshes.size());
  cardinality_training_data.resize(meshes.size());

  for (size_t m = 0; m < meshes.size(); m++)
  {
    features_training_data[m].resize(labels.size());
    adjacentparts_training_data[m].resize(labels.size());
    cardinality_training_data[m].resize(labels.size());

    for (size_t l = 0; l < labels.size(); l++)
    {
      cardinality_training_data[m][l] = meshes[m]->number_of_parts_per_label[l];

      if ( options.get("learn_tree_only", 0) == 1 )
      {
        if (ref_parts[m][l] != NULL)
        {
          ref_parts[m][l]->features = subvector( ref_parts[m][l]->features, 0, 5);
          selfAverageWithExistingVector(features_training_data[m][l], ref_parts[m][l]->features );
        }
      }
      else
      {
        if (ref_parts[m][l] != NULL)
          selfAverageWithExistingVector(features_training_data[m][l], ref_parts[m][l]->features);
      }
    }

    for (size_t l = 0; l < labels.size(); l++)
    {
      adjacentparts_training_data[m][l].resize(labels.at(l)->adj_labels.size(), 0);

      for (size_t a = 0; a < labels.at(l)->adj_labels.size(); a++)
      {
        MeshPart * part = ref_parts[m][l];

        if (part != NULL)
        {
          for (size_t pa = 0; pa < part->adjacent_parts.size(); pa++)
          {
            if (part->adjacent_parts[pa]->label == this->labels.at(l)->adj_labels[a])
            {
              if (!deactivate_adjacencies)
                adjacentparts_training_data[m][l][a]++;
            }
          }
        }
      }
    }
  } // end of loop over meshes
}


void ShapeBNet::train(SNL::Options const & _options)
{
  if ( features_training_data.empty() )
  {
    std::cout << "You should provide training data with setTrainingData before calling train()! Your call to train() did not do anything" << std::endl;
    std::cerr << "You should provide training data with setTrainingData before calling train()! Your call to train() did not do anything" << std::endl;
    return;
  }
  options.merge(_options);
  size_t min_root_domain_size = options.get("min_root_domain_size", 2);
  size_t max_root_domain_size = options.get("max_root_domain_size", 12);
  size_t min_add_layout_domain_size = options.get("min_add_layout_domain_size", 0);
  size_t max_add_layout_domain_size = options.get("max_add_layout_domain_size", 5);
  size_t num_max_em_iterations = options.get("num_max_em_iterations", 30);

  size_t rr = 0;
  SNL_VECTOR( SNL_VECTOR(SNL::Node *)* ) best_nodes;
  for (size_t root_hidden_node_domain_size = min_root_domain_size; root_hidden_node_domain_size <= max_root_domain_size; root_hidden_node_domain_size++, rr++)
  {
    //if (options.get("learn_tree_only", 0) == 1)
    //min_add_layout_domain_size = max( min_add_layout_domain_size, (size_t)floor( (float)root_hidden_node_domain_size / 2.0f) );
    SNL_VECTOR( size_t ) add_layout_domain_size( labels.size(), min_add_layout_domain_size );
    for (size_t l = 0; l < add_layout_domain_size.size(); l++)
    {
      add_layout_domain_size[l] = min( add_layout_domain_size[l], labels.at(l)->parts.size()-1 );
    }
    init(root_hidden_node_domain_size, add_layout_domain_size, true);
    learn(num_max_em_iterations);
    std::cout << "***Current model for this root domain size is considered the best. ( cs=" << root_node->cheesemanstutz << " )***" << std::endl;
    best_nodes.push_back(this->nodes);
    float previous_cheesemanstutz = root_node->cheesemanstutz;

    for (size_t l = 0; l < labels.size(); l++)
    {
      for (int k = min_add_layout_domain_size; k < min((int)max_add_layout_domain_size, (int)root_hidden_node_domain_size-2); k++)
      {
        add_layout_domain_size[l]++;
        if ( add_layout_domain_size[l] > labels.at(l)->parts.size()/2 )
        {
          break;
        }
        init(root_hidden_node_domain_size, add_layout_domain_size, true);
        learn(num_max_em_iterations);

        if ( root_node->cheesemanstutz > previous_cheesemanstutz )
        {
          add_layout_domain_size[l]--;
          internal_clean();
          break;
        }
        else
        {
          internal_clean( best_nodes[ rr ] );
          previous_cheesemanstutz = root_node->cheesemanstutz;
          std::cout << "***Current model for this root domain size is the best. ( cs=" << root_node->cheesemanstutz << " )***" << std::endl;
          best_nodes[ rr ] = this->nodes;
        }
      }
    } // end of loop over each layout node
  }

	std::cout << "Selecting best model...";	
  SNL_VECTOR(SNL::Node *)* best_of_the_best_nodes = best_nodes[0];
  rr = 0;
  for (size_t root_hidden_node_domain_size = min_root_domain_size; root_hidden_node_domain_size <= max_root_domain_size; root_hidden_node_domain_size++, rr++)
  {
    if ( ((RootNode*)*best_nodes[rr]->begin())->cheesemanstutz <= ((RootNode*)*best_of_the_best_nodes->begin())->cheesemanstutz)
    {
      best_of_the_best_nodes = best_nodes[rr];
    }
    else
    {
      internal_clean( best_nodes[ rr ] );
    }
  }
	
	std::cout << "Done" << std::endl;
  init(best_of_the_best_nodes);
  std::cout << "------ Initializing structure search with the best found model --------- " << std::endl;
  learn(num_max_em_iterations, true);
}


// structure search
void ShapeBNet::learn(size_t EM_iterations, bool full_structure_search)
{
  std::cout << "Learning model with root domain size " << root_node->domain_size << ", conf. domain size: ";
  for (size_t l = 0; l < labels.size(); l++)
    std::cout << layout_nodes[l]->domain_size << ", ";
  std::cout << std::endl;

  bool learn_tree_only = options.get("learn_tree_only", 0);
  if (learn_tree_only)
  {
    EM(EM_iterations);
    return;
  }
  float T_0 = options.get("wishart_hyperparameter_beta", 1e-4);
  size_t num_max_parents = options.get("max_parents_lateral", 3);

  // clean-up any previous default connections
  for (size_t n = 0; n < cardinality_nodes.size(); n++)
  {
    for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
    {
      adjacencynumparts_nodes[a]->parents_CardinalityNode.clear();
      adjacencynumparts_nodes[a]->registerParents();
    }
  }


  float bestcheesemanstutz = FLT_MAX;
  // structure search between cardinality nodes and adjacency nodes
  for (size_t n = 0; n < cardinality_nodes.size(); n++)
  {
    for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
    {
      if (cardinality_nodes[n]->label != adjacencynumparts_nodes[a]->label)
        continue;

      std::cout << "Connecting cardinality node of " << cardinality_nodes[n]->label->name << " to child adj. node of " <<  adjacencynumparts_nodes[a]->label->name << "-" << adjacencynumparts_nodes[a]->adj_label->name << std::endl;
      adjacencynumparts_nodes[a]->parents_CardinalityNode.push_back(cardinality_nodes[n]);
      size_t adjn = labels.at(n)->adj_labels[ adjacencynumparts_nodes[a]->adj_label_pos ]->id;
      std::cout << "Connecting cardinality node of " << cardinality_nodes[adjn]->label->name << " to child adj. node of " <<  adjacencynumparts_nodes[a]->label->name << "-" << adjacencynumparts_nodes[a]->adj_label->name << std::endl;
      adjacencynumparts_nodes[a]->parents_CardinalityNode.push_back(cardinality_nodes[adjn]);
      adjacencynumparts_nodes[a]->registerParents();
      //EM( EM_iterations );
      //if (root_node->cheesemanstutz > bestcheesemanstutz)
      //{
        //  std::cout << "WORSE." << std::endl;
      //  adjacencynumparts_nodes[a]->parents_CardinalityNode.pop_back();
      //  adjacencynumparts_nodes[a]->parents_CardinalityNode.pop_back();
      //  adjacencynumparts_nodes[a]->registerParents();
      //}
      //else
      //{
        //  std::cout << "BETTER." << std::endl;
      //  bestcheesemanstutz = root_node->cheesemanstutz;
      //}
    }
  }
  EM(EM_iterations);
  bestcheesemanstutz = root_node->cheesemanstutz;

  if (!full_structure_search)
    return;

  // structure search between cardinality nodes
  while (true)
  {
    size_t best_parent_n = boost::numeric::bounds<int>::highest(), best_child_n = boost::numeric::bounds<int>::highest();
    float largestdiff = 0.0;

    for (size_t n = 0; n < cardinality_nodes.size(); n++)
    {
      for (size_t n2 = 0; n2 < cardinality_nodes.size(); n2++)
      {
        if (n == n2)
          continue;

        if (cardinality_nodes[n]->domain_size == 1 || cardinality_nodes[n2]->domain_size == 1)
          continue;

        if (cardinality_nodes[n2]->parents_CardinalityNode.size() >= num_max_parents)
          continue;

        bool incompatible = false;

        for (size_t pp = 0; pp < cardinality_nodes[n2]->parents_CardinalityNode.size(); pp++)
        {
          if (cardinality_nodes[n2]->parents_CardinalityNode[pp] == cardinality_nodes[n])
          {
            incompatible = true;
            break;
          }
        }

        if (incompatible)
          continue;

        cardinality_nodes[n2]->parents_CardinalityNode.push_back(cardinality_nodes[n]);
        cardinality_nodes[n2]->registerParents();

        if (!topologicalSort())
        {
          cardinality_nodes[n2]->parents_CardinalityNode.pop_back();
          cardinality_nodes[n2]->registerParents();
          continue;
        }

        EM(1);

        float lets_be_less_strict_with_card_nodes = root_node->cheesemanstutz - T_0 * fabs(root_node->cheesemanstutz);
        if (bestcheesemanstutz - lets_be_less_strict_with_card_nodes > largestdiff)
        {
          largestdiff = bestcheesemanstutz - lets_be_less_strict_with_card_nodes;
          best_parent_n = n;
          best_child_n = n2;
        }

        cardinality_nodes[n2]->parents_CardinalityNode.pop_back();
        cardinality_nodes[n2]->registerParents();
      }
    }

    if (largestdiff == 0.0)
      break;

    std::cout << "Connecting card. node of " << cardinality_nodes[best_child_n]->label->name << " to parent card. node of " <<  cardinality_nodes[best_parent_n]->label->name << std::endl;
    cardinality_nodes[best_child_n]->parents_CardinalityNode.push_back(cardinality_nodes[best_parent_n]);
    cardinality_nodes[best_child_n]->registerParents();
    EM(1);
    bestcheesemanstutz = root_node->cheesemanstutz;
  }

  EM(1);
  bestcheesemanstutz = root_node->cheesemanstutz;
  root_node->old_Pr = root_node->Pr;
  for (size_t l = 0; l < labels.size(); l++)
  {
    layout_nodes[l]->old_Pl = layout_nodes[l]->Pl;
    layout_nodes[l]->old_Plr = layout_nodes[l]->Plr;
  }


  // structure search between feature nodes
  while (true)
  {
    size_t best_parent_n = boost::numeric::bounds<int>::highest(), best_child_n = boost::numeric::bounds<int>::highest();
    float largestdiff = 0.0;

    for (size_t g = 0; g < gaussian_nodes.size(); g++)
    {
      for (size_t g2 = 0; g2 < gaussian_nodes.size(); g2++)
      {
        if (g == g2)
          continue;

        if (gaussian_nodes[g2]->parents_gaussians.size() >= num_max_parents)
          continue;

        bool incompatible = false;

        for (size_t m = 0; m < meshes.size(); m++)
        {
          if (!features_training_data[m][g2].empty() && features_training_data[m][g].empty())
          {
            incompatible = true;
            break;
          }
        }

        for (size_t pp = 0; pp < gaussian_nodes[g2]->parents_gaussians.size(); pp++)
        {
          if (gaussian_nodes[g2]->parents_gaussians[pp] == gaussian_nodes[g])
          {
            incompatible = true;
            break;
          }
        }

        if (incompatible)
        {
          continue;
        }

        gaussian_nodes[g2]->parents_gaussians.push_back(gaussian_nodes[g]);

        if (!topologicalSort())
        {
          gaussian_nodes[g2]->parents_gaussians.pop_back();
          continue;
        }

        EM(1);

        float lets_be_less_strict_with_g_nodes = root_node->cheesemanstutz - T_0 * fabs(root_node->cheesemanstutz);
        if (bestcheesemanstutz - lets_be_less_strict_with_g_nodes > largestdiff)
        {
          largestdiff = bestcheesemanstutz - lets_be_less_strict_with_g_nodes;
          best_parent_n = g;
          best_child_n = g2;
        }
        gaussian_nodes[g2]->parents_gaussians.pop_back();
        root_node->Pr = root_node->old_Pr;
        for (size_t l = 0; l < labels.size(); l++)
        {
          layout_nodes[l]->Pl = layout_nodes[l]->old_Pl;
          layout_nodes[l]->Plr = layout_nodes[l]->old_Plr;
        }
      }
    }

    if (largestdiff == 0.0)
      break;

    std::cout << "Connecting gaussian node of " << gaussian_nodes[best_child_n]->layout_node->label->name << " to parent gaussian node of " <<  gaussian_nodes[best_parent_n]->layout_node->label->name << std::endl;
    gaussian_nodes[best_child_n]->parents_gaussians.push_back(gaussian_nodes[best_parent_n]);
    gaussian_nodes[best_child_n]->registerParents(); // parents are not used during training for gaussian nodes, only used for inference
    EM(1);
    bestcheesemanstutz = root_node->cheesemanstutz;
    root_node->Pr = root_node->old_Pr;

    for (size_t l = 0; l < labels.size(); l++)
    {
      layout_nodes[l]->Pl = layout_nodes[l]->old_Pl;
      layout_nodes[l]->Plr = layout_nodes[l]->old_Plr;
    }
  }

  EM(1);
  bestcheesemanstutz = root_node->cheesemanstutz;
  root_node->old_Pr = root_node->Pr;
  for (size_t l = 0; l < labels.size(); l++)
  {
    layout_nodes[l]->old_Pl = layout_nodes[l]->Pl;
    layout_nodes[l]->old_Plr = layout_nodes[l]->Plr;
  }
}


void ShapeBNet::EM(size_t EM_iterations)
{
  std::cout << "Running EM..." << std::endl;
  root_node->num_indep_parameters = 0;
  root_node->cheesemanstutz = FLT_MAX; // will be rewritten
  root_node->loglik = 0.0; // will be rewritten
  float prev_loglik = -FLT_MAX;
  SNL_VECTOR(float) logPevm;
  bool consider_empty_clusters = options.get("consider_empty_clusters", true);
  size_t early_termination_iterations = boost::numeric::bounds<int>::highest();
  int previous_num_clusters = 0;

  for (size_t em_it = 0; em_it < min(EM_iterations, early_termination_iterations); em_it++)
  {
    M();
    logPevm = E();
    CheesemanStutzScore();

    if (!consider_empty_clusters)
      root_node->empty_clusters = 0;
    if (em_it == 0)
      previous_num_clusters = root_node->empty_clusters;
    if ( root_node->empty_clusters > previous_num_clusters )
    {
      std::cout << "Restarting..." << std::endl;
      root_node->Pr = root_node->old_Pr;
      for (size_t l = 0; l < labels.size(); l++)
      {
        layout_nodes[l]->Pl = layout_nodes[l]->old_Pl;
        layout_nodes[l]->Plr = layout_nodes[l]->old_Plr;
      }
      early_termination_iterations = em_it;
      em_it = -1;
      continue;
    }

    std::cout << "Iteration " << em_it << ", log likelihood=" << root_node->loglik << ", cs=" << root_node->cheesemanstutz << "/ ec=" << root_node->empty_clusters << std::endl;
    if (abs(prev_loglik - root_node->loglik) < EPSILON && em_it > EM_iterations/2)
      break;
    if (prev_loglik > root_node->loglik + 0.1f * fabs(root_node->loglik) + 0.1f || root_node->loglik == -FLT_MAX)
    {
      std::cout << " NUMERICAL ERRORS PREVENT CONVERGENCE, SETTING LOKLIK = -FLT_MAX " << std::endl;
      root_node->cheesemanstutz = FLT_MAX;
      root_node->loglik = -FLT_MAX;
      break;
    }
    prev_loglik = root_node->loglik;
    previous_num_clusters = root_node->empty_clusters;
  }
}


void ShapeBNet::CheesemanStutzScore()
{
  std::cout << "C" << std::endl;
  double MAP_PRIOR_PARAM = options.get("dirichlet_hyperparameter_alpha", 0.1);
  double T_0 = options.get("wishart_hyperparameter_beta", 1e-4);
  double NU = options.get("wishart_hyperparameter_nu",  0.01);
  double complete_loglik_theta = 0.0;
  double complete_loglik = 0.0;

  //////////////// complete_loglik ////////////////
  root_node->clearMS();

  for (size_t l = 0; l < labels.size(); l++)
  {
    layout_nodes[l]->clearMS();
    cardinality_nodes[l]->clearMS();
  }

  for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
  {
    adjacencynumparts_nodes[a]->clearMS();
  }

  SNL_VECTOR( SNL_VECTOR(double) ) MSl(labels.size());

  for (size_t l = 0; l < labels.size(); l++)
  {
    MSl[l].resize(layout_nodes[l]->domain_size, 0.0);
  }

  for (size_t m = 0; m < meshes.size(); m++)
  {
    for (size_t d = 0; d < root_node->domain_size; d++)
    {
      root_node->MS[d] += root_node->Pr[m][d];

      for (size_t l = 0; l < labels.size(); l++)
      {
        for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
        {
          layout_nodes[l]->MS[ layout_nodes[l]->ac2(d2, d) ] += layout_nodes[l]->Plr[m][d2][d];
          MSl[l][d2] += layout_nodes[l]->Plr[m][d2][d];
        }

        size_t dc = cardinality_nodes[l]->getValuePos(cardinality_training_data[m][l]);
        SNL_VECTOR(size_t) parent_values;
        parent_values.push_back(d);
        parent_values.push_back(cardinality_training_data[m][l] > 0);

        for (size_t p = 0; p < cardinality_nodes[l]->parents_CardinalityNode.size(); p++)
          parent_values.push_back(cardinality_nodes[l]->parents_CardinalityNode[p]->getValuePos(cardinality_training_data[m][ cardinality_nodes[l]->parents_CardinalityNode[p]->label->id ]));

        cardinality_nodes[l]->MS[ cardinality_nodes[l]->acn(dc, parent_values) ] += root_node->Pr[m][d];
      }
    }

    for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
    {
      size_t l = adjacencynumparts_nodes[a]->label->id;
      size_t adj_pos = adjacencynumparts_nodes[a]->adj_label_pos;
      size_t dc = adjacencynumparts_nodes[a]->getValuePos(adjacentparts_training_data[m][l][adj_pos]);

      for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
      {
        SNL_VECTOR(size_t) parent_values;
        parent_values.push_back(d2);

        for (size_t p = 0; p < adjacencynumparts_nodes[a]->parents_CardinalityNode.size(); p++)
        {
          parent_values.push_back(adjacencynumparts_nodes[a]->parents_CardinalityNode[p]->getValuePos(cardinality_training_data[m][ adjacencynumparts_nodes[a]->parents_CardinalityNode[p]->label->id ]));
        }

        adjacencynumparts_nodes[a]->MS[ adjacencynumparts_nodes[a]->acn(dc, parent_values) ] += layout_nodes[l]->Pl[m][d2];
      }
    }
  }

	complete_loglik += LogGamma( double(root_node->domain_size) * MAP_PRIOR_PARAM );
	complete_loglik -= LogGamma( double(root_node->domain_size) * MAP_PRIOR_PARAM + (double)meshes.size() );
  for (size_t d = 0; d < root_node->domain_size; d++)
  {
    complete_loglik += LogGamma(MAP_PRIOR_PARAM + root_node->MS[d]) - LogGamma(MAP_PRIOR_PARAM);

    for (size_t l = 0; l < labels.size(); l++)
    {
      double tmp = double(layout_nodes[l]->domain_size) * MAP_PRIOR_PARAM;
      complete_loglik += LogGamma(tmp) - LogGamma(tmp + root_node->MS[d]);

      for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
      {
        complete_loglik += LogGamma(MAP_PRIOR_PARAM + layout_nodes[l]->MS[ layout_nodes[l]->ac2(d2, d) ]) - LogGamma(MAP_PRIOR_PARAM);
      }
    }
  }

  for (size_t l = 0; l < labels.size(); l++)
  {
    SNL_VECTOR(double) sumMS(cardinality_nodes[l]->MS.size(), 0.0);

    for (size_t i = 0; i < cardinality_nodes[l]->MS.size(); i++)
    {
      SNL_VECTOR(size_t) v;
      size_t n = cardinality_nodes[l]->getValues(i, v);

      if ((v[1] == 0 && cardinality_nodes[l]->values[n] > 0) || (v[1] == 1 && cardinality_nodes[l]->values[n] == 0))
        continue;

      complete_loglik += LogGamma(MAP_PRIOR_PARAM + cardinality_nodes[l]->MS[i]) - LogGamma(MAP_PRIOR_PARAM);
      sumMS[ cardinality_nodes[l]->acn(0, v) ] += cardinality_nodes[l]->MS[i];
    }

    for (size_t i = 0; i < cardinality_nodes[l]->MS.size(); i++)
    {
      SNL_VECTOR(size_t) v;
      size_t n = cardinality_nodes[l]->getValues(i, v);

      if (n == 0)
      {
        double tmp = double(cardinality_nodes[l]->domain_size) * MAP_PRIOR_PARAM;
        complete_loglik += LogGamma(tmp) - LogGamma(tmp + sumMS[ cardinality_nodes[l]->acn(0, v) ]);
      }
    }
  }

  for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
  {
    SNL_VECTOR( SNL_VECTOR(size_t) ) X_values(adjacencynumparts_nodes[a]->parents_CardinalityNode.size());

    for (size_t p = 0; p < adjacencynumparts_nodes[a]->parents_CardinalityNode.size(); p++)
    {
      X_values[p] = adjacencynumparts_nodes[a]->parents_CardinalityNode[p]->values;
    }

    SNL_VECTOR(double) sumMS(adjacencynumparts_nodes[a]->MS.size(), 0.0);

    for (size_t i = 0; i < adjacencynumparts_nodes[a]->MS.size(); i++)
    {
      SNL_VECTOR(size_t) v;
      size_t n = adjacencynumparts_nodes[a]->getValues(i, v);
      bool cont = false;

      for (size_t vi = 1; vi < v.size(); vi++)
        if (X_values[vi - 1][ v[vi] ] == 0 && adjacencynumparts_nodes[a]->values[n] > 0)
          cont = true;

        if (v[0] == 0 && adjacencynumparts_nodes[a]->values[n] > 0)
          cont = true;

        if (cont)
          continue;

        complete_loglik += LogGamma(MAP_PRIOR_PARAM + adjacencynumparts_nodes[a]->MS[i]) - LogGamma(MAP_PRIOR_PARAM);
      sumMS[ adjacencynumparts_nodes[a]->acn(0, v) ] += adjacencynumparts_nodes[a]->MS[i];
    }

    for (size_t i = 0; i < adjacencynumparts_nodes[a]->MS.size(); i++)
    {
      SNL_VECTOR(size_t) v;
      size_t n = adjacencynumparts_nodes[a]->getValues(i, v);

      if (n == 0)
      {
        double tmp = double(adjacencynumparts_nodes[a]->domain_size) * MAP_PRIOR_PARAM;
        complete_loglik += LogGamma(tmp) - LogGamma(tmp + sumMS[ adjacencynumparts_nodes[a]->acn(0, v) ]);
      }
    }
  }


  for (size_t g = 0; g < gaussian_nodes.size(); g++)
  {
    size_t l = gaussian_nodes[g]->layout_node->label->id;
    double kk = 0.0;

    for (size_t m = 0; m < features_training_data.size(); m++)
      if (!features_training_data[m][l].empty())
        kk++;

      for (size_t d2 = 1; d2 < layout_nodes[l]->domain_size; d2++)
      {
        double dim = (double)gaussian_nodes[l]->gaussians[d2].covariance.numRows();
        complete_loglik += (-dim * MSl[l][d2] / 2.0) * log(2.0 * M_PI);
        complete_loglik += (dim / 2.0) * log(NU / (NU + MSl[l][d2]));
        complete_loglik += logcdk(dim, dim) - logcdk(dim, dim + MSl[l][d2]);
        complete_loglik += (dim / 2.0) * dim * log(kk * T_0);
        complete_loglik += -((dim + MSl[l][d2]) / 2.0) * log(gaussian_nodes[g]->gaussians[d2].determinant_covariance + 1e-300);

        if (!gaussian_nodes[l]->gaussians[d2].b_0.empty())
        {
          size_t cci = 0, ccy = 0;

          for (size_t p = 0; p < gaussian_nodes[g]->parents_gaussians.size(); p++)
          {
            double dim = (double)gaussian_nodes[g]->parents_gaussians[p]->gaussians[1].covariance.numRows();
            ccy += (size_t)dim - 1;
            complete_loglik -= (-dim * MSl[l][d2] / 2.0) * log(2.0 * M_PI);
            complete_loglik -= (dim / 2.0) * log(NU / (NU + MSl[l][d2]));
            complete_loglik -= logcdk(dim, dim) - logcdk(dim, dim + MSl[l][d2]);
            complete_loglik -= (dim / 2.0) * dim * log(kk * T_0);
            complete_loglik -= -((dim + MSl[l][d2]) / 2.0) * log(determinant(subsqmatrix(gaussian_nodes[g]->gaussians[d2].covariance_xx, cci, ccy)) + 1e-300);
            cci += (size_t)dim;
            ccy = cci;
          }
        }
      }
  }


  //////////////// complete_loglik_theta ////////////////
  for (size_t m = 0; m < meshes.size(); m++)
  {
    for (size_t d = 0; d < root_node->domain_size; d++)
    {
      complete_loglik_theta += root_node->Pr[m][d] * log(root_node->cpt[d]);
    }

    for (size_t l = 0; l < labels.size(); l++)
    {
      for (size_t d = 0; d < root_node->domain_size; d++)
      {
        for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
        {
          complete_loglik_theta += layout_nodes[l]->Plr[m][d2][d] * log(layout_nodes[l]->cpt[ layout_nodes[l]->ac2(d2, d) ]);
        }
     }

      for (size_t d = 0; d < root_node->domain_size; d++)
      {
        size_t dc = cardinality_nodes[l]->getValuePos(cardinality_training_data[m][l]);
        SNL_VECTOR(size_t) parent_values;
        parent_values.push_back(d);
        parent_values.push_back(cardinality_training_data[m][l] > 0);

        for (size_t p = 0; p < cardinality_nodes[l]->parents_CardinalityNode.size(); p++)
          parent_values.push_back(cardinality_nodes[l]->parents_CardinalityNode[p]->getValuePos(cardinality_training_data[m][ cardinality_nodes[l]->parents_CardinalityNode[p]->label->id ]));

        complete_loglik_theta += root_node->Pr[m][d] * log(cardinality_nodes[l]->cpt[ cardinality_nodes[l]->acn(dc, parent_values) ]);
      }
    }

    for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
    {
      size_t l = adjacencynumparts_nodes[a]->label->id;
      size_t adj_pos = adjacencynumparts_nodes[a]->adj_label_pos;
      size_t dc = adjacencynumparts_nodes[a]->getValuePos(adjacentparts_training_data[m][l][adj_pos]);

      for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
      {
        if (d2 == 0 && adjacentparts_training_data[m][l][adj_pos] > 0)
          continue;

        SNL_VECTOR(size_t) parent_values;
        parent_values.push_back(d2);

        for (size_t p = 0; p < adjacencynumparts_nodes[a]->parents_CardinalityNode.size(); p++)
        {
          parent_values.push_back(adjacencynumparts_nodes[a]->parents_CardinalityNode[p]->getValuePos(cardinality_training_data[m][ adjacencynumparts_nodes[a]->parents_CardinalityNode[p]->label->id ]));
        }

        complete_loglik_theta += layout_nodes[l]->Pl[m][d2] * log(adjacencynumparts_nodes[a]->cpt[ adjacencynumparts_nodes[a]->acn(dc, parent_values) ]);
      }
    }

    for (size_t g = 0; g < gaussian_nodes.size(); g++)
    {
      size_t l = gaussian_nodes[g]->layout_node->label->id;

      for (size_t d2 = 1; d2 < layout_nodes[l]->domain_size; d2++)
      {
        if (!features_training_data[m][l].empty())
        {
          SNL_VECTOR(double) Fy = features_training_data[m][l];
          SNL_VECTOR(double) Fx;

          for (size_t p = 0; p < gaussian_nodes[l]->parents_gaussians.size(); p++)
          {
            size_t lx = gaussian_nodes[l]->parents_gaussians[p]->layout_node->label->id;
            Fx = concatenateVectors(Fx, features_training_data[m][lx]);
          }

          if (gaussian_nodes[l]->gaussians[d2].b_0.empty())
            complete_loglik_theta += layout_nodes[l]->Pl[m][d2] * log(gaussian_nodes[l]->gaussians[d2].evaluateGaussian(Fy));
          else
            complete_loglik_theta += layout_nodes[l]->Pl[m][d2] * log(gaussian_nodes[l]->gaussians[d2].evaluateCondGaussian(Fy, Fx));
        }
      }
    }
  }

  root_node->cheesemanstutz = complete_loglik + root_node->loglik - complete_loglik_theta;
  root_node->cheesemanstutz = -root_node->cheesemanstutz; // 'cause we consider the minimum due to aic, bic
  root_node->cheesemanstutz += double(root_node->empty_clusters) * (1.0 / EPSILON);
}


///////////////////// EM ////////////////
///////////////////// EM ////////////////
///////////////////// EM ////////////////
void ShapeBNet::M()
{
  double MAP_PRIOR_PARAM = options.get("dirichlet_hyperparameter_alpha", 0.1);
  double T_0 = options.get("wishart_hyperparameter_beta", 1e-4);
  double NU = options.get("wishart_hyperparameter_nu",  0.01);
  bool use_cpts_everywhere = options.get("use_cpts_everywhere", false);
  std::cout << "M";

  // ROOT NODE
  root_node->initialize();
  std::cout << "r";
  root_node->learn(MAP_PRIOR_PARAM);

  //// CONFIGURATION & CARDINALITY NODES CPT
  for (size_t l = 0; l < labels.size(); l++)
  {
    layout_nodes[l]->initialize();
    std::cout << "l" << l;
    layout_nodes[l]->learn(MAP_PRIOR_PARAM);

    cardinality_nodes[l]->initialize();
    std::cout << "c" << l;
    cardinality_nodes[l]->learn(cardinality_training_data, MAP_PRIOR_PARAM, use_cpts_everywhere);    // existence node will be ininitialized and learned here as well
  }

  //// ADJACENCY NODES CPT
  for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
  {
    std::cout << "a" << a;
    adjacencynumparts_nodes[a]->initialize();
    adjacencynumparts_nodes[a]->learn(adjacentparts_training_data, cardinality_training_data, MAP_PRIOR_PARAM, use_cpts_everywhere);
  }

  //// GAUSSIAN NODES LEARNING
  for (size_t g = 0; g < gaussian_nodes.size(); g++)
  {
    std::cout << "g" << g;

    if (gaussian_nodes[g]->learn(features_training_data, T_0, NU))
    {
      root_node->loglik = -FLT_MAX;
    }
  }
}


SNL_VECTOR(float) ShapeBNet::E()
{
  // for each mesh, compute probability P(data_l | c_l = j)                       [PdIl]
  //                for each label l and for each domain value of c_l
  //                compute probability P(c_l = j | r = i ) * P(data_l | c_l = j) [Plr]
  //                compute sum_j P(data_l | c_l = j) * P(data_l | c_l = j)       [sumPlr]
  std::cout << "E";
  root_node->empty_clusters = 0;
  SNL_VECTOR(float) logPevm;

  if (root_node->loglik != -FLT_MAX)
    root_node->loglik = 0.0;

  for (size_t m = 0; m < meshes.size(); m++)
  {
    SNL_VECTOR( SNL_VECTOR(double) ) PdIl(labels.size());               // L x C_l domain size
    SNL_VECTOR( SNL_VECTOR( SNL_VECTOR(double) ) ) Plr(labels.size());  // L x C_l domain size x root domain size
    SNL_VECTOR( SNL_VECTOR(double) ) sumPlr(labels.size());             // L x root domain size
    double Pev = 0.0;                                                   // normalization

    for (size_t l = 0; l < labels.size(); l++)
    {
      // compute PdIl
      PdIl[l].resize(layout_nodes[l]->domain_size, 0.0);
      Plr[l].resize(layout_nodes[l]->domain_size);
      sumPlr[l].resize(root_node->domain_size, 0.0);

      for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
      {
        // P(f|l)
        if (d2 == 0)     // value corresponding to no instance of label exists
        {
          if (features_training_data[m][l].empty())
            PdIl[l][d2] = 1.0;
          else
            PdIl[l][d2] = 0.0;
        }
        else
        {
          if (!features_training_data[m][l].empty())
          {
            SNL_VECTOR(double) Fy = features_training_data[m][l];
            SNL_VECTOR(double) Fx;

            for (size_t p = 0; p < gaussian_nodes[l]->parents_gaussians.size(); p++)
            {
              size_t lx = gaussian_nodes[l]->parents_gaussians[p]->layout_node->label->id;
              Fx = concatenateVectors(Fx, features_training_data[m][lx]);
            }

            if (gaussian_nodes[l]->gaussians[d2].b_0.empty())
              PdIl[l][d2] = gaussian_nodes[l]->gaussians[d2].evaluateGaussian(Fy);
            else
              PdIl[l][d2] = gaussian_nodes[l]->gaussians[d2].evaluateCondGaussian(Fy, Fx);
          }
        }

        // P(a|l)
        for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
        {
          if (adjacencynumparts_nodes[a]->label->id != l)  // l, not 1
            continue;

          size_t adjacentpart_value = adjacencynumparts_nodes[a]->getValuePos(adjacentparts_training_data[m][l][ adjacencynumparts_nodes[a]->adj_label_pos ]);
          SNL_VECTOR( size_t ) parent_adjacentpart_values;
          parent_adjacentpart_values.push_back(d2);

          for (size_t p = 0; p < adjacencynumparts_nodes[a]->parents_CardinalityNode.size(); p++)
            parent_adjacentpart_values.push_back(adjacencynumparts_nodes[a]->parents_CardinalityNode[p]->getValuePos(cardinality_training_data[m][ adjacencynumparts_nodes[a]->parents_CardinalityNode[p]->label->id ]));

          PdIl[l][d2] *= adjacencynumparts_nodes[a]->cpt[ adjacencynumparts_nodes[a]->acn(adjacentpart_value, parent_adjacentpart_values) ];
        }

        // P(e|l)
        //PdIl[l][d2] *= cardinality_nodes[l]->e->cpt[ cardinality_nodes[l]->e->ac2( cardinality_training_data[m][l] > 0, d2 ) ];

        PdIl[l][d2] = max(PdIl[l][d2], SUPEREPSILON);
      } // end of loop over domain values of configuration node

      //if (DEBUG_TRAINING)
      //{
        //  std::cout << PdIl[l] << std::endl;
        //system("pause");
        //}

        // compute Plr
        for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
        {
          Plr[l][d2].resize(root_node->domain_size, 0.0);

          for (size_t d = 0; d < root_node->domain_size; d++)
            Plr[l][d2][d] = PdIl[l][d2] * layout_nodes[l]->cpt[layout_nodes[l]->ac2(d2, d)];
        }

        for (size_t d = 0; d < root_node->domain_size; d++)
          for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
            sumPlr[l][d] += Plr[l][d2][d];
    } // end of loop over each label


    for (size_t l = 0; l < labels.size(); l++)
    {
      for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
      {
        for (size_t d = 0; d < root_node->domain_size; d++)
        {
          layout_nodes[l]->Plr[m][d2][d] = root_node->cpt[d];

          for (size_t n = 0; n < cardinality_nodes.size(); n++)
          {
            size_t cardinality_value = cardinality_nodes[n]->getValuePos(cardinality_training_data[m][n]);
            SNL_VECTOR( size_t ) parent_values;
            parent_values.push_back(d);
            parent_values.push_back(cardinality_training_data[m][n] > 0);

            for (size_t p = 0; p < cardinality_nodes[n]->parents_CardinalityNode.size(); p++)
              parent_values.push_back(cardinality_nodes[n]->parents_CardinalityNode[p]->getValuePos(cardinality_training_data[m][ cardinality_nodes[n]->parents_CardinalityNode[p]->label->id ]));

            layout_nodes[l]->Plr[m][d2][d] *= cardinality_nodes[n]->cpt[ cardinality_nodes[n]->acn(cardinality_value, parent_values) ];
          }

          for (size_t l2 = 0; l2 < labels.size(); l2++)
          {
            if (l == l2)
              layout_nodes[l]->Plr[m][d2][d] *= Plr[l][d2][d];
            else
              layout_nodes[l]->Plr[m][d2][d] *= sumPlr[l2][d];
          }
        }
      }
    } // end of loop over each label

    for (size_t l = 0; l < labels.size(); l++)
    {
      for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
      {
        layout_nodes[l]->Pl[m][d2] = 0.0;

        for (size_t d = 0; d < root_node->domain_size; d++)
          layout_nodes[l]->Pl[m][d2] += layout_nodes[l]->Plr[m][d2][d];
      }
    }

    for (size_t d = 0; d < root_node->domain_size; d++)
    {
      root_node->Pr[m][d] = 0.0;

      for (size_t d2 = 0; d2 < layout_nodes[0]->domain_size; d2++)
        root_node->Pr[m][d] += layout_nodes[0]->Plr[m][d2][d];
    }

    for (size_t d = 0; d < root_node->domain_size; d++)
      Pev += root_node->Pr[m][d];

    for (size_t l = 0; l < labels.size(); l++)
      for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
        for (size_t d = 0; d < root_node->domain_size; d++)
          layout_nodes[l]->Plr[m][d2][d] /= Pev;

        for (size_t l = 0; l < labels.size(); l++)
          for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
            layout_nodes[l]->Pl[m][d2] /= Pev;

          for (size_t d = 0; d < root_node->domain_size; d++)
            root_node->Pr[m][d] /= Pev;


          logPevm.push_back(log(Pev));

        if (root_node->loglik != -FLT_MAX)
          root_node->loglik += log(Pev);
  } // end of loop over each mesh m


	style_id.resize( meshes.size() );
  root_node->most_likely_meshes.clear();
  root_node->most_likely_meshes.resize(root_node->domain_size);
  root_node->style_id.clear();
  root_node->style_id.resize( meshes.size() );
  for (size_t m = 0; m < meshes.size(); m++)
  {
    size_t maximum_pr_m = maximumId(root_node->Pr.getRow(m));
		style_id[m] = maximum_pr_m;
    root_node->most_likely_meshes[ maximum_pr_m ].push_back(meshes.at(m));
    root_node->style_id[m] = maximum_pr_m;
  }
  for (size_t l = 0; l < labels.size(); l++)
  {
    layout_nodes[l]->most_likely_meshes.clear();
    layout_nodes[l]->most_likely_meshes.resize(layout_nodes[l]->domain_size);
  }
  for (size_t m = 0; m < meshes.size(); m++)
  {
    for (size_t l = 0; l < labels.size(); l++)
    {
      size_t maximum_pr_m = maximumId(layout_nodes[l]->Pl.getRow(m));
      layout_nodes[l]->most_likely_meshes[ maximum_pr_m ].push_back(meshes.at(m));
    }
  }


  root_node->num_indep_parameters = 0;
  for (size_t d = 0; d < root_node->domain_size; d++)
    if (root_node->most_likely_meshes[ d ].empty())
      root_node->empty_clusters += 1000;

    for (size_t l = 0; l < labels.size(); l++)
    {
      for (size_t d2 = 1; d2 < layout_nodes[l]->domain_size; d2++)
      {
        if (layout_nodes[l]->most_likely_meshes[ d2 ].empty())
        {
          root_node->num_indep_parameters -= (int)gaussian_nodes[l]->num_param(d2);
          root_node->empty_clusters++; // empty clusters for everything, but in root node they are more important
        }
      }
    }
    root_node->num_indep_parameters += (int)root_node->num_param();
    for (size_t n = 0; n < labels.size(); n++)
    {
      root_node->num_indep_parameters += (int)cardinality_nodes[n]->num_param();
      root_node->num_indep_parameters += (int)layout_nodes[n]->num_param();
      root_node->num_indep_parameters += (int)gaussian_nodes[n]->num_param();
    }
    for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
    {
      root_node->num_indep_parameters += (int)adjacencynumparts_nodes[a]->num_param();
    }

    return logPevm;
}


////////////////////////////////////////////////////////////
//// SAVE / LOAD BNET ///
////////////////////////////////////////////////////////////
void ShapeBNet::printStats(std::ostream & os)
{
  if ( root_node == NULL || layout_nodes.empty() )
  {
    os << "bnet is empty! Load a network or learn it from data. " << std::endl;
    return;
  }
  if (root_node->most_likely_meshes.empty())
  {
    this->E();
  }
  os << "Root domain size " << root_node->domain_size << ", conf. domain size: ";
  for (size_t l = 0; l < labels.size(); l++)
    os << layout_nodes[l]->domain_size << ", ";
  os << "EM resulted in:" << std::endl;
  os << "log likelihood=" << root_node->loglik << ", cs-ml=" << root_node->cheesemanstutz << std::endl;
  os << "#parameters:" << root_node->num_indep_parameters << std::endl;

  os << "Most likely meshes per style: " << std::endl;
  for (size_t d = 0; d < root_node->domain_size; d++)
  {
    os << "Style " << d << ": ";
    for (size_t m = 0; m < root_node->most_likely_meshes[d].size(); m++)
      os << root_node->most_likely_meshes[d][m]->id << " (" << root_node->most_likely_meshes[d][m]->filename << ")" << ", ";
    os << std::endl;
  }

  os << "Most likely *initial* meshes per style: " << std::endl;
  for (size_t d = 0; d < root_node->domain_size; d++)
  {
    os << "Style " << d << ": ";
    for (size_t m = 0; m < root_node->most_likely_initial_meshes[d].size(); m++)
      os << root_node->most_likely_initial_meshes[d][m]->id << ", ";
    os << std::endl;
  }

  os << "Prob of meshes per style: " << std::endl;
  for (size_t m = 0; m < meshes.size(); m++)
  {
    os << m << " " << meshes.at(m)->filename << " ";
    for (size_t d = 0; d < root_node->domain_size; d++)
    {
      os << root_node->Pr[m][d] << " ";
    }
    os << std::endl;
  }

  os << "Most likely meshes per label: " << std::endl;
  for (size_t l = 0; l < labels.size(); l++)
  {
    os << "Label " << l << " " << labels.at(l)->name << std::endl;
    for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
    {
      os << "Style " << d2 << ": ";
      for (size_t m = 0; m < layout_nodes[l]->most_likely_meshes[d2].size(); m++)
        os << layout_nodes[l]->most_likely_meshes[d2][m]->id << " (" << layout_nodes[l]->most_likely_meshes[d2][m]->filename << ")" << ", ";
      os << std::endl;
    }

    for (size_t m = 0; m < meshes.size(); m++)
    {
      os << m << " " << meshes.at(m)->filename << " ";
      for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
      {
        os << layout_nodes[l]->Pl[m][d2] << " ";
      }
      os << std::endl;
    }
  }

  os << "Most likely *initial* meshes per label: " << std::endl;
  for (size_t l = 0; l < labels.size(); l++)
  {
    os << "Label " << l << " " << labels.at(l)->name << std::endl;
    for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
    {
      os << "Style " << d2 << ": ";
      for (size_t m = 0; m < layout_nodes[l]->most_likely_initial_meshes[d2].size(); m++)
        os << layout_nodes[l]->most_likely_initial_meshes[d2][m]->id << " (" << layout_nodes[l]->most_likely_initial_meshes[d2][m]->filename << ")" << ", ";

      os << std::endl;
    }
  }

  os << "Root CPT: " << std::endl;
  os << root_node->cpt << std::endl;
  for (size_t l = 0; l < labels.size(); l++)
  {
    os << labels.at(l)->name << " configuration CPT: " << std::endl;
    os << layout_nodes[l]->cpt << std::endl;
    os << labels.at(l)->name << " cardinality CPT: " << std::endl;
    os << cardinality_nodes[l]->cpt << std::endl;
  }

  for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
  {
    os << "Adjacency node cpt of adj. label" << labels.at(adjacencynumparts_nodes[a]->label->id)->name;
    os << "-" << labels.at(adjacencynumparts_nodes[a]->adj_label->id)->name << std::endl;
    os <<  adjacencynumparts_nodes[a]->cpt;
  }

  for (size_t l = 0; l < labels.size(); l++)
  {
    for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
    {
      os << labels.at(l)->name << " gaussian, style: " << d2 << std::endl;
      os << "b0:" << gaussian_nodes[l]->gaussians[d2].b_0 << std::endl;
      os << "b:" <<  gaussian_nodes[l]->gaussians[d2].b << std::endl;
      os << "cov:" <<  gaussian_nodes[l]->gaussians[d2].covariance << std::endl;
      os << "bcov:" <<  gaussian_nodes[l]->gaussians[d2].bak_covariance << std::endl;
      os << "prec:" <<  gaussian_nodes[l]->gaussians[d2].inverse_covariance << std::endl;
      os << "det:" <<  gaussian_nodes[l]->gaussians[d2].determinant_covariance << std::endl;
      os << "mean:" <<  gaussian_nodes[l]->gaussians[d2].mean << std::endl;
      os << "bmean:" <<  gaussian_nodes[l]->gaussians[d2].bak_mean << std::endl;
      os << "#points:" <<  gaussian_nodes[l]->gaussians[d2].num_data_points << std::endl;
    }
  }
}


void ShapeBNet::save(std::string const & out_path) const
{
  ofstream bnet_file( out_path.c_str() );

  bnet_file << root_node->add_layout_domain_size << std::endl;
  bnet_file << root_node->domain_size << std::endl;
  bnet_file << root_node->cpt << std::endl;

  for (size_t l = 0; l < labels.size(); l++)
  {
    bnet_file << layout_nodes[l]->cpt << std::endl;
    bnet_file << cardinality_nodes[l]->cpt << std::endl;
  }

  for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
  {
    bnet_file << adjacencynumparts_nodes[a]->cpt << std::endl;
  }

  for (size_t l = 0; l < labels.size(); l++)
  {
    for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
    {
      bnet_file << gaussian_nodes[l]->gaussians[d2].b_0 << std::endl;
      bnet_file << gaussian_nodes[l]->gaussians[d2].b << std::endl;
      bnet_file << gaussian_nodes[l]->gaussians[d2].covariance << std::endl;
      bnet_file << gaussian_nodes[l]->gaussians[d2].bak_covariance << std::endl;
      bnet_file << gaussian_nodes[l]->gaussians[d2].inverse_covariance << std::endl;
      bnet_file << gaussian_nodes[l]->gaussians[d2].determinant_covariance << std::endl;
      bnet_file << gaussian_nodes[l]->gaussians[d2].mean << std::endl;
      bnet_file << gaussian_nodes[l]->gaussians[d2].bak_mean << std::endl;
      bnet_file << gaussian_nodes[l]->gaussians[d2].rank << std::endl;
      bnet_file << gaussian_nodes[l]->gaussians[d2].num_data_points << std::endl;
    }
  }

  for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
  {
    bnet_file << adjacencynumparts_nodes[a]->parents_CardinalityNode.size() << std::endl;
    for (size_t p = 0; p < adjacencynumparts_nodes[a]->parents_CardinalityNode.size(); p++)
      bnet_file << adjacencynumparts_nodes[a]->parents_CardinalityNode[p]->label->id << std::endl;
  }

  for (size_t l = 0; l < labels.size(); l++)
  {
    bnet_file << gaussian_nodes[l]->parents_gaussians.size() << std::endl;
    for (size_t p = 0; p < gaussian_nodes[l]->parents_gaussians.size(); p++)
      bnet_file << gaussian_nodes[l]->parents_gaussians[p]->layout_node->label->id << std::endl;
  }

  for (size_t l = 0; l < labels.size(); l++)
  {
    bnet_file << cardinality_nodes[l]->parents_CardinalityNode.size() << std::endl;
    for (size_t p = 0; p < cardinality_nodes[l]->parents_CardinalityNode.size(); p++)
      bnet_file << cardinality_nodes[l]->parents_CardinalityNode[p]->label->id << std::endl;
  }

  bnet_file.close();

  std::cout << "Saved bnet in " << out_path << std::endl;
}


void ShapeBNet::load(std::string const & in_path)
{
  ifstream bnet_file(in_path.c_str());

  if (!bnet_file.good())
  {
    std::cout << "Could not find the bnet file: " << in_path << std::endl;
    exit(-1);
  }

  internal_clean();
  SNL_VECTOR( size_t ) _add_layout_domain_size;
  size_t _root_domain_size;
  bnet_file >> _add_layout_domain_size;
  bnet_file >> _root_domain_size;
  init(_root_domain_size, _add_layout_domain_size, false);

  bnet_file >> root_node->cpt;
  std::cout << "Read root node cpt." << std::endl;

  for (size_t l = 0; l < labels.size(); l++)
  {
    bnet_file >> layout_nodes[l]->cpt;
    bnet_file >> cardinality_nodes[l]->cpt;
    std::cout << "Read layout and cardinality node cpt for label" << labels.at(l)->name << std::endl;
  }


  for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
  {
    bnet_file >> adjacencynumparts_nodes[a]->cpt;
    std::cout << "Read adjacency nodes cpt for label" << labels.at(adjacencynumparts_nodes[a]->label->id)->name;
    std::cout << "-" << labels.at(adjacencynumparts_nodes[a]->adj_label->id)->name << std::endl;
  }

  for (size_t l = 0; l < labels.size(); l++)
  {
    gaussian_nodes[l]->gaussians.resize(layout_nodes[l]->domain_size);

    for (size_t d2 = 0; d2 < layout_nodes[l]->domain_size; d2++)
    {
      bnet_file >> gaussian_nodes[l]->gaussians[d2].b_0;
      bnet_file >> gaussian_nodes[l]->gaussians[d2].b;
      bnet_file >> gaussian_nodes[l]->gaussians[d2].covariance;
      selfPruneValues( gaussian_nodes[l]->gaussians[d2].covariance, 0.0 );
      bnet_file >> gaussian_nodes[l]->gaussians[d2].bak_covariance;
      bnet_file >> gaussian_nodes[l]->gaussians[d2].inverse_covariance;
      selfPruneValues( gaussian_nodes[l]->gaussians[d2].inverse_covariance, 0.0 );
      bnet_file >> gaussian_nodes[l]->gaussians[d2].determinant_covariance;
      bnet_file >> gaussian_nodes[l]->gaussians[d2].mean;
      bnet_file >> gaussian_nodes[l]->gaussians[d2].bak_mean;
      bnet_file >> gaussian_nodes[l]->gaussians[d2].rank;
      bnet_file >> gaussian_nodes[l]->gaussians[d2].num_data_points;
    }

    std::cout << "Read gaussian node data for label" << labels.at(l)->name << std::endl;
  }

  for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
  {
    size_t ps;

    bnet_file >> ps;
    adjacencynumparts_nodes[a]->parents_CardinalityNode.resize(ps);
    for (size_t p = 0; p < adjacencynumparts_nodes[a]->parents_CardinalityNode.size(); p++)
    {
      size_t parent_id;
      bnet_file >> parent_id;
      adjacencynumparts_nodes[a]->parents_CardinalityNode[p] = cardinality_nodes[ parent_id ];
    }
    adjacencynumparts_nodes[a]->registerParents();
    for (size_t p = 0; p < adjacencynumparts_nodes[a]->parents_CardinalityNode.size(); p++)
    {
      std::cout << "Connected adjacency node of labels" << labels.at(adjacencynumparts_nodes[a]->label->id)->name;
      std::cout << "-" << labels.at(adjacencynumparts_nodes[a]->adj_label->id)->name << std::endl;
      std::cout << " to parent card. node " << adjacencynumparts_nodes[a]->parents_CardinalityNode[p]->label->name << std::endl;
    }
  }

  for (size_t l = 0; l < labels.size(); l++)
  {
    size_t ps;
    bnet_file >> ps;
    gaussian_nodes[l]->parents_gaussians.resize(ps);
    for (size_t p = 0; p < gaussian_nodes[l]->parents_gaussians.size(); p++)
    {
      size_t parent_id;
      bnet_file >> parent_id;
      gaussian_nodes[l]->parents_gaussians[p] = gaussian_nodes[ parent_id ];
      std::cout << "Connected gaussian node of label" << labels.at(l)->name << " to parent node of label " << labels.at(parent_id)->name << std::endl;
    }
    gaussian_nodes[l]->registerParents(); // parents are not used during training for gaussian nodes, only used for inference
  }

  for (size_t l = 0; l < labels.size(); l++)
  {
    size_t ps;
    bnet_file >> ps;
    cardinality_nodes[l]->parents_CardinalityNode.resize(ps);

    for (size_t p = 0; p < cardinality_nodes[l]->parents_CardinalityNode.size(); p++)
    {
      size_t parent_id;
      bnet_file >> parent_id;
      cardinality_nodes[l]->parents_CardinalityNode[p] = cardinality_nodes[ parent_id ];
      std::cout << "Connected card. node of label" << labels.at(l)->name << " to parent node of label " << labels.at(parent_id)->name << std::endl;
    }

    cardinality_nodes[l]->e->initialize();
    cardinality_nodes[l]->e->learn(); // deterministic cpt
    cardinality_nodes[l]->registerParents();
  }
  bnet_file.close();

  std::cout << "Loaded bnet from " << in_path << std::endl;

}


///////////////////////////////////////
/// INFERENCE ///
///////////////////////////////////////

void ShapeBNet::setObservation(SNL::Node* node, const double value)
{}


void ShapeBNet::setObservation(SNL::Node* node, const std::vector<double>& value)
{}


void ShapeBNet::clearObservation(SNL::Node* node)
{}


void ShapeBNet::clearAllObservations()
{}


// used for inference only, call only ONCE in the beginning.
void ShapeBNet::init( SNL::Options const & _options )
{}


// call once after evidence updated
void ShapeBNet::doInference()
{}


void ShapeBNet::doAnalyticSpecificInference()
{}





MeshPart* ShapeBNet::getMeshRefPart( size_t m, size_t l )
{
	MeshPart *ref_part = NULL;
	if ( m != -1)
	{
		for (size_t pm = 0; pm < meshes.at(m)->parts.size(); pm++)
		{
			MeshPart *part = parts.at( meshes.at(m)->parts[pm]->mesh_part_id );
			if ( part->label == labels.at(l) && part->sublabel_id == 1 )
			{
				ref_part = part;
				break;
			}
		}				
	}
	return ref_part;
}


vector<ShapeBNetSample> ShapeBNet::getAllInstances(char *output_filename, int limit_output_samples)
{
	double MAP_PRIOR_PARAM = ( internal_map_multiplier / (double)meshes.size() ); //(double)max<size_t>(meshes->size(), 100) );
	const int MULTIPLIER_PER_STYLE = 50;
	vector< ShapeBNetSample > samples;
	vector< vector<int> >     final_assignments;
	vector< vector<int> >     selected_components;
	vector< int >							num_samples_per_style( root_node->domain_size, 0 );
	vector< int >							num_times_part_used( parts.size(), 0 );
	std::priority_queue<TreeNode> pq;	
	if (!topologicalSort())
	{
		std::cout << "Internal error: learned bnet had cycles, although it should not" << std::endl;
		exit(-1);
	}
	//vector< vector< vector< double > > > stdfeatures_training_data = getStandardizedFeatures();

	size_t counter = 0;
	treemap[ std::make_pair('R', 0) ] = counter;
	for (size_t l = 0; l < labels.size(); l++)
	{
		counter++;
		treemap[ std::make_pair('L', l) ] = counter;
	}
	for (size_t l = 0; l < labels.size(); l++)
	{
		counter++;
		treemap[ std::make_pair('C', ordered_cardinality_nodes[l]->label->id) ] = counter;
	}
	for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
	{
		counter++;
		treemap[ std::make_pair('A', a) ] = counter;
	}
//#if DO_NOT_USE_SLOTS == 0
//	for (size_t a = 0; a < adjacencynumslots_nodes.size(); a++)
//	{
//		counter++;
//		treemap[ KEY('B', a) ] = counter;
//	}
//#endif
	for (size_t l = 0; l < labels.size(); l++)
	{
		counter++;
		treemap[ std::make_pair('G', ordered_gaussian_nodes[l]->layout_node->label->id) ] = counter;
	}

	time_t start,end;
	time (&start);

	pq.push( TreeNode() ); // empty root node
	while (!pq.empty()) 
	{		
		if ( final_assignments.size() > limit_output_samples )
			break;

		if ( limit_output_samples != INT_MAX )
		{
			time (&end);
			double dif = difftime (end,start);
			if (dif > double( 60*60 ) )
				break;
		}

		TreeNode t = pq.top();
		pq.pop();
		vector<TreeNode> neighbors;
		switch(t.ntype)
		{
		case '-':
			{
				for (size_t i = 0; i < root_node->domain_size; i++)
				{
					if ( root_node->most_likely_meshes[i].size() <= 1 )
						continue;
					neighbors.push_back( TreeNode(t, root_node->cpt[i], 'R', 0, i) );
				}
			}
			break;
		case 'R':
			{
				if (t.getLogLikValue() < log(1e-5))
					continue;
				for (size_t i = 0; i < layout_nodes[0]->domain_size; i++)
				{
					double prob = layout_nodes[0]->cpt[ layout_nodes[0]->ac2( i, t.value ) ];
					if ( prob <= MAP_PRIOR_PARAM )
						continue;
					neighbors.push_back( TreeNode(t, prob, 'L', 0, i) );
				}
			}
			break;
		case 'L':
			{
				if ( num_samples_per_style[ t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] ] > MULTIPLIER_PER_STYLE*root_node->most_likely_meshes[ t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] ].size() )
				{
					continue;
				}
				if (t.getLogLikValue() < log(1e-5))
					continue;
				size_t nid = t.nid+1;
				if (nid < labels.size())
				{
					for (size_t i = 0; i < layout_nodes[nid]->domain_size; i++)
					{
						double prob = layout_nodes[nid]->cpt[ layout_nodes[ nid ]->ac2( i, t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ]  ) ];
						if ( prob <= MAP_PRIOR_PARAM )
							continue;					
						neighbors.push_back( TreeNode(t, prob, 'L', nid, i) );
					}
				}
				else
				{
					if (t.getLogLikValue() < log(1e-6))
						continue;
					vector<size_t> parent_values;
					parent_values.push_back( t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] );	
					parent_values.push_back( t.partial_assignment[ treemap[ std::make_pair('L', ordered_cardinality_nodes[0]->label->id) ] ] > 0 );	
					for (size_t i = 0; i < ordered_cardinality_nodes[0]->domain_size; i++)
					{					
						double prob = ordered_cardinality_nodes[0]->cpt[ ordered_cardinality_nodes[0]->acn( i, parent_values ) ];
						if ( prob <= MAP_PRIOR_PARAM )
							continue;
						if ( t.partial_assignment[ treemap[ std::make_pair('L', ordered_cardinality_nodes[0]->label->id) ] ] == 0 && ordered_cardinality_nodes[0]->values[i] > 0 )
							continue;
						if ( t.partial_assignment[ treemap[ std::make_pair('L', ordered_cardinality_nodes[0]->label->id) ] ] > 0 && ordered_cardinality_nodes[0]->values[i] == 0 )
							continue;
						neighbors.push_back( TreeNode(t, prob, 'C', 0, i) );
					}
				}
			}
			break;
		case 'C':
			{
				if ( num_samples_per_style[ t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] ] > MULTIPLIER_PER_STYLE*root_node->most_likely_meshes[ t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] ].size() )
				{
					continue;
				}
				size_t nid = t.nid+1;
				if (nid < labels.size())
				{
					if (t.getLogLikValue() < log(1e-6))
						continue;
					vector<size_t> parent_values;
					parent_values.push_back( t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] );	
					parent_values.push_back( t.partial_assignment[ treemap[ std::make_pair('L', ordered_cardinality_nodes[nid]->label->id) ] ] > 0 );					
					for (size_t p = 0; p < ordered_cardinality_nodes[nid]->parents_CardinalityNode.size(); p++)
						parent_values.push_back( t.partial_assignment[ treemap[ std::make_pair('C', ordered_cardinality_nodes[nid]->parents_CardinalityNode[p]->label->id) ] ] );
					for (size_t i = 0; i < ordered_cardinality_nodes[nid]->domain_size; i++)
					{					
						double prob = ordered_cardinality_nodes[nid]->cpt[ ordered_cardinality_nodes[nid]->acn( i, parent_values ) ];					
						if ( prob <= MAP_PRIOR_PARAM )
							continue;
						if ( t.partial_assignment[ treemap[ std::make_pair('L', ordered_cardinality_nodes[nid]->label->id) ] ] == 0 && ordered_cardinality_nodes[nid]->values[i] > 0 )
							continue;
						if ( t.partial_assignment[ treemap[ std::make_pair('L', ordered_cardinality_nodes[nid]->label->id) ] ] > 0 && ordered_cardinality_nodes[nid]->values[i] == 0 )
							continue;
						neighbors.push_back( TreeNode(t, prob, 'C', nid, i) );
					}
				}
				else
				{
					if (t.getLogLikValue() < log(1e-8))
						continue;
					vector<size_t> parent_values;				
					parent_values.push_back( t.partial_assignment[ treemap[ std::make_pair('L', adjacencynumparts_nodes[0]->label->id) ] ] );	
					for (size_t p = 0; p < adjacencynumparts_nodes[0]->parents_CardinalityNode.size(); p++)
					{
						parent_values.push_back( t.partial_assignment[ treemap[ std::make_pair('C', adjacencynumparts_nodes[0]->parents_CardinalityNode[p]->label->id) ] ] );	
					}
					for (size_t i = 0; i < adjacencynumparts_nodes[0]->domain_size; i++)
					{					
						double prob = adjacencynumparts_nodes[0]->cpt[ adjacencynumparts_nodes[0]->acn( i, parent_values ) ];
						if ( prob <= MAP_PRIOR_PARAM )
							continue;
						neighbors.push_back( TreeNode(t, prob, 'A', 0, i) );
					}
				}
			}
			break;
		case 'A':
			{
				if ( num_samples_per_style[ t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] ] > MULTIPLIER_PER_STYLE*root_node->most_likely_meshes[ t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] ].size() )
				{
					continue;
				}
				size_t nid = t.nid+1;
				if (nid < adjacencynumparts_nodes.size())
				{
					if (t.getLogLikValue() < log(1e-8))
						continue;
					vector<size_t> parent_values;				
					parent_values.push_back( t.partial_assignment[ treemap[ std::make_pair('L', adjacencynumparts_nodes[nid]->label->id) ] ] );	
					for (size_t p = 0; p < adjacencynumparts_nodes[nid]->parents_CardinalityNode.size(); p++)
					{
						parent_values.push_back( t.partial_assignment[ treemap[ std::make_pair('C', adjacencynumparts_nodes[nid]->parents_CardinalityNode[p]->label->id) ] ] );	
					}
					for (size_t i = 0; i < adjacencynumparts_nodes[nid]->domain_size; i++)
					{					
						double prob = adjacencynumparts_nodes[nid]->cpt[ adjacencynumparts_nodes[nid]->acn( i, parent_values ) ];
						if ( prob <= MAP_PRIOR_PARAM )
							continue;
						neighbors.push_back( TreeNode(t, prob, 'A', nid, i) );
					}
				}
//#if DO_NOT_USE_SLOTS == 0
//				else
//				{
//					if (t.getLogLikValue() < log(.5*1e-7))
//						continue;
//					vector<size_t> parent_values;				
//					parent_values.push_back( t.partial_assignment[ treemap[ KEY('L', adjacencynumslots_nodes[0]->label->id) ] ] );	
//					for (size_t p = 0; p < adjacencynumslots_nodes[0]->parents_CardinalityNode.size(); p++)
//					{
//						parent_values.push_back( t.partial_assignment[ treemap[ KEY('C', adjacencynumslots_nodes[0]->parents_CardinalityNode[p]->label->id) ] ] );	
//					}
//					for (size_t i = 0; i < adjacencynumslots_nodes[0]->domain_size; i++)
//					{					
//						double prob = adjacencynumslots_nodes[0]->cpt[ adjacencynumslots_nodes[0]->acn( i, parent_values ) ];
//						if ( prob <= MAP_PRIOR_PARAM )
//							continue;
//						neighbors.push_back( TreeNode(t, prob, 'B', 0, i) );
//					}
//				}
//			}
//			break;
//		case 'B':
//			{
//				size_t nid = t.nid+1;
//				if (nid < adjacencynumslots_nodes.size())
//				{
//					if (t.getLogLikValue() < log(.5*1e-7))
//						continue;
//					vector<size_t> parent_values;				
//					parent_values.push_back( t.partial_assignment[ treemap[ KEY('L', adjacencynumslots_nodes[nid]->label->id) ] ] );	
//					for (size_t p = 0; p < adjacencynumslots_nodes[nid]->parents_CardinalityNode.size(); p++)
//					{
//						parent_values.push_back( t.partial_assignment[ treemap[ KEY('C', adjacencynumslots_nodes[nid]->parents_CardinalityNode[p]->label->id) ] ] );	
//					}
//					for (size_t i = 0; i < adjacencynumslots_nodes[nid]->domain_size; i++)
//					{					
//						double prob = adjacencynumslots_nodes[nid]->cpt[ adjacencynumslots_nodes[nid]->acn( i, parent_values ) ];
//						if ( prob <= MAP_PRIOR_PARAM )
//							continue;
//						neighbors.push_back( TreeNode(t, prob, 'B', nid, i) );
//					}
//				}
//#endif
				else
				{		
					if (t.getLogLikValue() < log(1e-8))
						continue;
					size_t d2 = t.partial_assignment[ treemap[ std::make_pair('L', ordered_gaussian_nodes[0]->layout_node->label->id) ] ];	
					if ( d2 == 0 )
					{
						neighbors.push_back( TreeNode(t, 0.99, 'G', 0, -1) );
						break;
					}
					for (size_t m = 0; m < meshes.size(); m++)
					{					
						if ( style_id[ m ] != t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] )
							continue;
						bool incompatible = false;
						for (size_t p = 0; p < ordered_gaussian_nodes[0]->parents_gaussians.size(); p++)
						{
							size_t lx = ordered_gaussian_nodes[0]->parents_gaussians[p]->layout_node->label->id;
							if ( t.partial_assignment[ treemap[ std::make_pair('G', lx) ] ] == -1 )
								incompatible = true;
						}
						if (incompatible)
							continue;

						int adjpart_penalty = 0;
						int adjslot_penalty = 0;
						int adjlabels = 0;
						for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
						{
							if ( adjacencynumparts_nodes[a]->label->id != ordered_gaussian_nodes[0]->layout_node->label->id )
								continue;
							adjlabels++;
							if (adjacencynumparts_nodes[a]->values[t.partial_assignment[ treemap[ std::make_pair('A', a) ] ] ] != adjacentparts_training_data[m][adjacencynumparts_nodes[a]->label->id][adjacencynumparts_nodes[a]->adj_label_pos])
								adjpart_penalty++;
//#if DO_NOT_USE_SLOTS == 0
//							if (adjacencynumslots_nodes[a]->values[t.partial_assignment[ treemap[ KEY('B', a) ] ] ] != adjacentslots_training_data[m][adjacencynumslots_nodes[a]->label->id][adjacencynumslots_nodes[a]->adj_label_pos])
//								adjslot_penalty++;
//#endif
						}
						//if ( adjpart_penalty > 0 )
						//	continue;

						vector<double> Fy = features_training_data[m][ ordered_gaussian_nodes[0]->layout_node->label->id ];
						double prob = ordered_gaussian_nodes[0]->gaussians[d2].evaluateGaussian( Fy );
						prob = ordered_gaussian_nodes[0]->gaussians[d2].getRoughProbability( prob, (double)ordered_gaussian_nodes[0]->gaussians[d2].covariance.numRows() );
						if ( prob < SUPEREPSILON )
							continue;
						neighbors.push_back( TreeNode(t, 0.99+min(prob, 0.01), 'G', 0, m, double(adjslot_penalty+adjpart_penalty)) );
					}
				}
			}
			break;
		case 'G':
			{
				bool part_used_too_many_times = false;
				for (size_t l = 0; l < labels.size(); l++)
				{
					if ( treemap[ std::make_pair('G', l) ] >= t.partial_assignment.size() )
						continue;
					size_t mesh_id = t.partial_assignment[ treemap[ std::make_pair('G', l) ] ];
					if ( mesh_id != -1 )
					{
						MeshPart *ref_part = getMeshRefPart( mesh_id, l );							
						if (ref_part == NULL)
						{
							part_used_too_many_times = true;
							break;
						}
						if ( num_times_part_used[ ref_part->id ] > MULTIPLIER_PER_STYLE )
						{
							part_used_too_many_times = true;
							break;
						}
					}
				}
				if ( part_used_too_many_times )
					continue;
				if ( num_samples_per_style[ t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] ] > MULTIPLIER_PER_STYLE*root_node->most_likely_meshes[ t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] ].size() )
				{
					continue;
				}
				//if ( t.getAdjPenalty() /  (double( adjacencynumslots_nodes.size() )+1e-30) > .01 )
				//	continue;

				size_t nid = t.nid+1;
				if (nid < gaussian_nodes.size())
				{
					if (t.getLogLikValue() < log(1e-8))
						continue;
					size_t d2 = t.partial_assignment[ treemap[ std::make_pair('L', ordered_gaussian_nodes[nid]->layout_node->label->id) ] ];	
					if ( d2 == 0 )
					{
						neighbors.push_back( TreeNode(t, 0.99, 'G', nid, -1) );
						break;
					}
					for (size_t m = 0; m < meshes.size(); m++)
					{					
						if ( style_id[ m ] != t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] )
							continue;
						bool incompatible = false;
						for (size_t p = 0; p < ordered_gaussian_nodes[nid]->parents_gaussians.size(); p++)
						{
							size_t lx = ordered_gaussian_nodes[nid]->parents_gaussians[p]->layout_node->label->id;
							if ( t.partial_assignment[ treemap[ std::make_pair('G', lx) ] ] == -1 )
								incompatible = true;
						}
						if (incompatible)
							continue;

						int adjpart_penalty = 0;
						int adjslot_penalty = 0;
						int adjlabels = 0;
						for (size_t a = 0; a < adjacencynumparts_nodes.size(); a++)
						{
							if ( adjacencynumparts_nodes[a]->label->id != ordered_gaussian_nodes[nid]->layout_node->label->id )
								continue;
							adjlabels++;
							if (adjacencynumparts_nodes[a]->values[t.partial_assignment[ treemap[ std::make_pair('A', a) ] ] ] != adjacentparts_training_data[m][adjacencynumparts_nodes[a]->label->id][adjacencynumparts_nodes[a]->adj_label_pos])
								adjpart_penalty++;
//#if DO_NOT_USE_SLOTS == 0
//							if (adjacencynumslots_nodes[a]->values[t.partial_assignment[ treemap[ std::make_pair('B', a) ] ] ] != adjacentslots_training_data[m][adjacencynumslots_nodes[a]->label->id][adjacencynumslots_nodes[a]->adj_label_pos])
//								adjslot_penalty++;
//#endif
						}
						//if ( adjpart_penalty > 0 )
						//	continue;

						vector<double> Fy = features_training_data[m][ ordered_gaussian_nodes[nid]->layout_node->label->id ];
						vector<double> Fx;
						for (size_t p = 0; p < ordered_gaussian_nodes[nid]->parents_gaussians.size(); p++)
						{
							size_t lx = ordered_gaussian_nodes[nid]->parents_gaussians[p]->layout_node->label->id;
							Fx = concatenateVectors( Fx, features_training_data[ t.partial_assignment[ treemap[ std::make_pair('G', lx) ] ] ][lx] );
						}
						double prob = 0.0;
						if ( ordered_gaussian_nodes[nid]->gaussians[d2].b_0.empty() )
							prob = ordered_gaussian_nodes[nid]->gaussians[d2].evaluateGaussian( Fy );
						else
							prob = ordered_gaussian_nodes[nid]->gaussians[d2].evaluateCondGaussian( Fy, Fx );
						prob = ordered_gaussian_nodes[nid]->gaussians[d2].getRoughProbability( prob, (double)ordered_gaussian_nodes[nid]->gaussians[d2].covariance.numRows() );
						if ( prob < SUPEREPSILON )
							continue;
						neighbors.push_back( TreeNode(t, 0.99+min(prob, 0.01), 'G', nid, m, double(adjslot_penalty+adjpart_penalty)) );
					}
				}
				else
				{					
					//if ( t.getAdjPenalty() /  (double( adjacencynumslots_nodes.size() )+1e-30) > .01 )
					//	continue;
					vector<int> selected_components_i = std::vector<int>(t.partial_assignment.end()-labels.size(), t.partial_assignment.end() );
					if ( find( selected_components.begin(), selected_components.end(), selected_components_i ) != selected_components.end() )
						continue;
					selected_components.push_back( selected_components_i );

					ShapeBNetSample s;
					s.prob = exp( t.getLogLikValue() );
					s.bnet = this;
					s.scales.resize( labels.size() );
					s.root_node_value = t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ];
					for (size_t l = 0; l < labels.size(); l++)
					{
						s.cardinality_nodes_value.push_back( cardinality_nodes[l]->values[ t.partial_assignment[ treemap[ std::make_pair('C', labels.at(l)->id) ] ] ] );
						s.use_mesh_ids.push_back( t.partial_assignment[ treemap[ std::make_pair('G', labels.at(l)->id) ] ] );
						s.scales[l].push_back( gaussian_nodes[l]->gaussians[ t.partial_assignment[ treemap[ std::make_pair('L', labels.at(l)->id) ] ] ].bak_mean[0] );
						s.scales[l].push_back( gaussian_nodes[l]->gaussians[ t.partial_assignment[ treemap[ std::make_pair('L', labels.at(l)->id) ] ] ].bak_mean[1] );
						s.scales[l].push_back( gaussian_nodes[l]->gaussians[ t.partial_assignment[ treemap[ std::make_pair('L', labels.at(l)->id) ] ] ].bak_mean[2] );
						s.scales[l].push_back( gaussian_nodes[l]->gaussians[ t.partial_assignment[ treemap[ std::make_pair('L', labels.at(l)->id) ] ] ].bak_covariance[0][0] );
						s.scales[l].push_back( gaussian_nodes[l]->gaussians[ t.partial_assignment[ treemap[ std::make_pair('L', labels.at(l)->id) ] ] ].bak_covariance[0][1] );
						s.scales[l].push_back( gaussian_nodes[l]->gaussians[ t.partial_assignment[ treemap[ std::make_pair('L', labels.at(l)->id) ] ] ].bak_covariance[0][2] );
						s.scales[l].push_back( gaussian_nodes[l]->gaussians[ t.partial_assignment[ treemap[ std::make_pair('L', labels.at(l)->id) ] ] ].bak_covariance[1][0] );
						s.scales[l].push_back( gaussian_nodes[l]->gaussians[ t.partial_assignment[ treemap[ std::make_pair('L', labels.at(l)->id) ] ] ].bak_covariance[1][1] );
						s.scales[l].push_back( gaussian_nodes[l]->gaussians[ t.partial_assignment[ treemap[ std::make_pair('L', labels.at(l)->id) ] ] ].bak_covariance[1][2] );
						s.scales[l].push_back( gaussian_nodes[l]->gaussians[ t.partial_assignment[ treemap[ std::make_pair('L', labels.at(l)->id) ] ] ].bak_covariance[2][0] );
						s.scales[l].push_back( gaussian_nodes[l]->gaussians[ t.partial_assignment[ treemap[ std::make_pair('L', labels.at(l)->id) ] ] ].bak_covariance[2][1] );
						s.scales[l].push_back( gaussian_nodes[l]->gaussians[ t.partial_assignment[ treemap[ std::make_pair('L', labels.at(l)->id) ] ] ].bak_covariance[2][2] );
					}

					float distToClosestMesh = FLT_MAX, distToClosestSample = FLT_MAX;
					for (size_t m = 0; m < meshes.size(); m++)
					{
						if ( style_id[m] != t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] )
							continue;
						float distToMesh = 0.0f, dweight = 0.0;
						for (size_t l = 0; l < labels.size(); l++)
						{
							if ( s.use_mesh_ids[l] == -1 || features_training_data[m][l].empty() )
								continue;
							float weight1 = features_training_data[ s.use_mesh_ids[l] ][l][0]*features_training_data[ s.use_mesh_ids[l] ][l][1]*features_training_data[ s.use_mesh_ids[l] ][l][2];
							float weight2 = features_training_data[m][l][0]*features_training_data[m][l][1]*features_training_data[m][l][2];
							distToMesh += (weight1+weight2) * dist( features_training_data[ s.use_mesh_ids[l] ][l], features_training_data[m][l] ) / float( features_training_data[m][l].size() );
							dweight += weight1+weight2;
						}
						distToMesh /= (dweight + SUPEREPSILON);
						if ( distToMesh < distToClosestMesh )
						{
							distToClosestMesh = distToMesh;
						}
					}
					if ( distToClosestMesh < .01 )
					{
						//std::cout << "Sample distance to closest mesh too low: " << distToClosestMesh << std::endl;		
						continue;
					}

					for (size_t m = 0; m < samples.size(); m++)
					{
						if ( samples[m].root_node_value != t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] )
							continue;
						float distToMesh = 0.0f, dweight = 0.0;
						for (size_t l = 0; l < labels.size(); l++)
						{
							if ( s.use_mesh_ids[l] == -1 ||  samples[m].use_mesh_ids[l] == -1 )
								continue;
							float weight1 = features_training_data[ s.use_mesh_ids[l] ][l][0]*features_training_data[ s.use_mesh_ids[l] ][l][1]*features_training_data[ s.use_mesh_ids[l] ][l][2];
							float weight2 = features_training_data[ samples[m].use_mesh_ids[l] ][l][0]*features_training_data[ samples[m].use_mesh_ids[l] ][l][1]*features_training_data[ samples[m].use_mesh_ids[l] ][l][2];
							distToMesh += (weight1+weight2)*dist( features_training_data[ s.use_mesh_ids[l] ][l], features_training_data[ samples[m].use_mesh_ids[l] ][l] ) / float( features_training_data[ samples[m].use_mesh_ids[l] ][l].size() );
							dweight += weight1+weight2;
						}
						distToMesh /= (dweight + SUPEREPSILON);
						if ( distToMesh < distToClosestSample )
						{
							distToClosestSample = distToMesh;
						}
					}
					if ( distToClosestSample < .01 )
					{
						//std::cout << "Sample distance to closest sample too low: " << distToClosestSample << std::endl;		
						continue;
					}	


					MeshCreator m( s, labels, parts, meshes,  vector< vector< MeshPart* > >(), NULL, 0);
					if (!m.valid_sampled_mesh)
					{
						if ( m.problematic_part != NULL  )
						{
							num_times_part_used[ m.problematic_part->id ]++;
						}
						continue;
					}

					for (size_t l = 0; l < labels.size(); l++)
					{
						if ( s.use_mesh_ids[l] != -1 )
						{
							MeshPart *ref_part = getMeshRefPart( s.use_mesh_ids[l], l );							
							if (ref_part == NULL )
								continue;
							num_times_part_used[ ref_part->id ]++;
						}
					}

					s.distToClosestMesh = distToClosestMesh;
					s.distToClosestSample = distToClosestSample;
					num_samples_per_style[ t.partial_assignment[ treemap[ std::make_pair('R', 0) ] ] ]++;
					final_assignments.push_back( t.partial_assignment );
					samples.push_back( s );
					std::cout << "FA size: " << final_assignments.size() << ", ll=" << t.getLogLikValue() << " " << distToClosestMesh << " " << distToClosestSample << " " << t.partial_assignment << std::endl; 
				} // end of else for whether "G" is last
			} // end of case for "G"
			break;
		default:
			std::cerr << "internal error - node type during 'getAllInstances' is unknown!" << std::endl;
			exit(-1);
		}


		for (size_t i = 0; i < neighbors.size(); i++) 
		{

			if ( neighbors[i].getLogLikValue() < log(1e-8) )
				continue;
			pq.push( neighbors[i] );
		}
	}
	
	std::cout << num_samples_per_style << std::endl;

	if ( output_filename != NULL)
	{
		ofstream data_out( output_filename );
		data_out << final_assignments << std::endl;
		for (size_t m = 0; m < samples.size(); m++)
		{
			data_out << samples[m].prob << " " << samples[m].distToClosestMesh << " " << samples[m].distToClosestSample << std::endl;
		}
		data_out.close();
	}

	std::cout << "Done traversing tree - check and press any key to continue" << std::endl;

	return samples;
}



} // namespace S12
} // namespace SNL
