// Copyright (c) 2013-2020, SIB - Swiss Institute of Bioinformatics and
//                          Biozentrum - University of Basel
// 
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// 
//   http://www.apache.org/licenses/LICENSE-2.0
// 
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.


#include <algorithm>
#include <queue>
#include <limits>
#include <map>
#include <random>

#include <promod3/core/graph_minimizer.hh>
#include <promod3/core/tree.hh>
#include <promod3/core/runtime_profiling.hh>
#include <promod3/core/enumerator.hh>
#include <promod3/core/message.hh>


namespace{

////////////////////////////////////////////////////////////////
//FUNCTIONS AND DATA STRUCTURES FOR THE TREE SOLVING ALGORITHM//
////////////////////////////////////////////////////////////////

typedef promod3::core::TreeBag<int> TBag;

// calculates a linear idx for a certain combination in the lset
struct LSetIdxCalculator{

  LSetIdxCalculator() { }

  LSetIdxCalculator(const std::vector<unsigned short>& num_candidates) {
    lset_size = num_candidates.size();
    for(int i = 0; i < lset_size; ++i) {
      int temp = 1;
      for(int j = i + 1; j < lset_size; ++j) {
        temp *= num_candidates[j];
      }
      idx_helper.push_back(temp);
    }
    idx_helper.push_back(1);
  }

  inline int GetLIdx(const std::vector<unsigned short>& indices) const {
    int idx = 0;
    for(int i = 0; i < lset_size; ++i) {
      idx += idx_helper[i] * indices[i];
    }
    return idx;
  }

  inline int GetIdxHelper(int idx) {
    return idx_helper[idx];
  }

  std::vector<int> idx_helper;
  int lset_size;
};


struct BagData{

  // members of the bag that are also present in parent bag
  std::vector<int> lset;
  // members of the bag that are not present in parent bag
  std::vector<int> rset;

  // number of solutions for every entry in lset/rset
  uint num_l_combinations;
  uint num_r_combinations;
  std::vector<unsigned short> num_l_solutions;
  std::vector<unsigned short> num_r_solutions;

  // optimal solution in r set given a certain combination in l
  // the index for a certain combination in l can be estimated with
  // l_set_idx_calculator
  // assuming an l_idx of x, the according optimal rset data starts
  // at x * rset.size()
  std::vector<unsigned short> optimal_rset_combination;

  // the according score
  std::vector<Real> optimal_r_score;

  // calculates an index in optimal_r_combination/optimal_r_score
  // given a certain combination in l
  LSetIdxCalculator lset_idx_calculator;

  // the bag data indices for the children attached to this bag
  std::vector<int> children_indices;
  int num_children;

  // super complicated mapping structure of getting stuff out from the children
  // the idea is, that we can calculate the lset idx in several steps in the
  // enumeration procedure...

  // for every child we have a vector of with indices describing the
  // elements in the current lset being part of the childrens lset
  std::vector<std::vector<int> > lset_child_l_indices;
  // the number it has to be multiplied to construct the index for
  // that particular child lset
  std::vector<std::vector<int> > lset_child_l_index_helpers;
  // store the size of the stuff above for every child
  std::vector<int> num_lset_child_l;

  // equivalent for the current rset
  std::vector<std::vector<int> > rset_child_l_indices;
  std::vector<std::vector<int> > rset_child_l_index_helpers;
  std::vector<int> num_rset_child_l; 

  // this data structure will be altered during the calculation and is a
  // placeholder to query optimal solutions for certain lsets of the children
  std::vector<std::vector<int> > children_l_combinations;

  // self energies of the rset
  std::vector<std::vector<Real> > r_self_energies;

  // stuff for lr and rl pairwise energies
  std::vector<promod3::core::EMatXX> lr_energies;
  std::vector<int> lr_index_in_I;
  std::vector<int> lr_index_in_R;

  // stuff for rr pairwise energies
  std::vector<promod3::core::EMatXX> rr_energies;
  std::vector<int> rr_index_in_R_one;
  std::vector<int> rr_index_in_R_two;
}; 


// Fills only the bare minimum of data into the BagData vector and already
// estimates the complexity of the tree solving procedure. All other data
// gets filled in the FillBagData function
uint64_t FillRawBagData(const std::vector<TBag*>& traversal,
                        const std::vector<int>& num_active_solutions,
                        std::vector<BagData>& bag_data) {

  uint64_t complexity = 0;

  bag_data.clear();
  bag_data.resize(traversal.size());

  for(uint bag_idx = 0; bag_idx < traversal.size(); ++bag_idx) {

    TBag* bag = traversal[bag_idx];
    TBag* parent = traversal[bag_idx]->GetParent();

    // assign rset and lset
    if(parent == NULL) {
      // it's the root, lets shuffle all to rset
      bag_data[bag_idx].rset.assign(bag->members_begin(),
                                    bag->members_end());
    }
    else{
      for(TBag::const_member_iterator it = bag->members_begin(); 
          it != bag->members_end(); ++it) {
        if(parent->HasMember(*it)) bag_data[bag_idx].lset.push_back(*it);
        else bag_data[bag_idx].rset.push_back(*it);
      }
    }

    // figure out how many solutions there are
    bag_data[bag_idx].num_l_combinations = 1;
    bag_data[bag_idx].num_r_combinations = 1;

    for(std::vector<int>::iterator i = bag_data[bag_idx].lset.begin(); 
        i != bag_data[bag_idx].lset.end(); ++i) {
      bag_data[bag_idx].num_l_solutions.push_back(num_active_solutions[*i]);
      bag_data[bag_idx].num_l_combinations *= num_active_solutions[*i];
    }

    for(std::vector<int>::iterator i = bag_data[bag_idx].rset.begin(); 
        i != bag_data[bag_idx].rset.end(); ++i) {
      bag_data[bag_idx].num_r_solutions.push_back(num_active_solutions[*i]);
      bag_data[bag_idx].num_r_combinations *= num_active_solutions[*i];
    }

    complexity += (bag_data[bag_idx].num_l_combinations * 
                   bag_data[bag_idx].num_r_combinations);
  }

  return complexity;
}  


void FillBagData(const std::vector<TBag*>& traversal,
                 const std::vector<promod3::core::GMNode*>& nodes,
                 const std::vector<promod3::core::GMEdge*>& edges,
                 const std::vector<std::pair<int, int> >& edge_indices,
                 std::vector<BagData>& bag_data) {
  
  for(uint bag_idx = 0; bag_idx < traversal.size(); ++bag_idx) {

    TBag* bag = traversal[bag_idx];
    BagData& current_data = bag_data[bag_idx];
    int current_data_lsize = current_data.lset.size();
    int current_data_rsize = current_data.rset.size();

    // allocate required space for optimal r combination for every possible lset
    current_data.optimal_rset_combination.resize(current_data_rsize *
                                              current_data.num_l_combinations);    
   
    current_data.optimal_r_score.resize(current_data.num_l_combinations);

    // generate a new lset idxgenerator...
    current_data.lset_idx_calculator = 
    LSetIdxCalculator(current_data.num_l_solutions);

    // get all the children stuff right
    for(TBag::const_child_iterator it = bag->children_begin(); 
        it != bag->children_end(); ++it) {
      current_data.children_indices.push_back((*it)->GetIdx());
    }
    current_data.num_children = current_data.children_indices.size();

    // handle current lset
    current_data.lset_child_l_indices.resize(current_data.num_children);
    current_data.lset_child_l_index_helpers.resize(current_data.num_children);
    for(int i = 0; i < current_data_lsize; ++i) {
      int val = current_data.lset[i];
      // go through all children
      for(int j = 0; j < current_data.num_children; ++j) {
        int ch_idx = current_data.children_indices[j];
        // we can be sure of the children lsets being set due to the postorder
        // traversal of the tree
        const std::vector<int>& child_lset = bag_data[ch_idx].lset;
        std::vector<int>::const_iterator it = std::find(child_lset.begin(), 
                                                        child_lset.end(), 
                                                        val);
        if(it != child_lset.end()) {
          // its in the childs lset!
          int idx_in_child_l = it - child_lset.begin();
          current_data.lset_child_l_indices[j].push_back(i);
          int helper = 
          bag_data[ch_idx].lset_idx_calculator.GetIdxHelper(idx_in_child_l);
          current_data.lset_child_l_index_helpers[j].push_back(helper);
        }
      }
    }
    for(int i = 0; i < current_data.num_children; ++i) {
      int size = current_data.lset_child_l_indices[i].size();
      current_data.num_lset_child_l.push_back(size);
    }

    // do the same for the rset
    current_data.rset_child_l_indices.resize(current_data.num_children);
    current_data.rset_child_l_index_helpers.resize(current_data.num_children);
    for(int i = 0; i < current_data_rsize; ++i) {
      int val = current_data.rset[i];
      // go through all children
      for(int j = 0; j < current_data.num_children; ++j) {
        int ch_idx = current_data.children_indices[j];
        // we can be sure of the children lsets being set due to the postorder
        // traversal of the tree
        const std::vector<int>& child_lset = bag_data[ch_idx].lset;
        std::vector<int>::const_iterator it = std::find(child_lset.begin(), 
                                                        child_lset.end(), 
                                                        val);
        if(it != child_lset.end()) {
          // its in the childs lset!
          int idx_in_child_l = it - child_lset.begin();
          current_data.rset_child_l_indices[j].push_back(i);
          int helper = 
          bag_data[ch_idx].lset_idx_calculator.GetIdxHelper(idx_in_child_l);
          current_data.rset_child_l_index_helpers[j].push_back(helper);
        }
      }
    }
    for(int i = 0; i < current_data.num_children; ++i) {
      int size = current_data.rset_child_l_indices[i].size();
      current_data.num_rset_child_l.push_back(size);
    }

    // prepare the lset combinations
    for(std::vector<int>::iterator i = current_data.children_indices.begin();
        i != current_data.children_indices.end(); ++i) {
      int size = bag_data[*i].lset.size();
      current_data.children_l_combinations.push_back(std::vector<int>(size,0));
    }

    // fill the rset self energies
    current_data.r_self_energies.resize(current_data_rsize);
    for(int i = 0; i < current_data_rsize; ++i) {
      promod3::core::GMNode* n = nodes[current_data.rset[i]];
      n->FillActiveSelfEnergies(current_data.r_self_energies[i]);
    }

    // Search for all pairs in lset / rset, that have an edge

    std::vector<promod3::core::GMEdge*> lr_edges;

    for(int i = 0; i < current_data_lsize; ++i) {
      for(int j = 0; j < current_data_rsize; ++j) {

        std::pair<int,int> idx_pair = std::make_pair(current_data.lset[i],
                                                     current_data.rset[j]); 
        std::vector<std::pair<int,int> >::const_iterator idx_it =
        std::find(edge_indices.begin(), edge_indices.end(), idx_pair); 

        if(idx_it != edge_indices.end()) {
          int edge_idx = idx_it - edge_indices.begin();
          lr_edges.push_back(edges[edge_idx]);
          current_data.lr_index_in_I.push_back(i);
          current_data.lr_index_in_R.push_back(j);
          continue;
        }
        idx_pair = std::make_pair(current_data.rset[j], current_data.lset[i]); 
        idx_it = std::find(edge_indices.begin(), edge_indices.end(), idx_pair);
        if(idx_it != edge_indices.end()) {
          int edge_idx = idx_it - edge_indices.begin();
          lr_edges.push_back(edges[edge_idx]);
          current_data.lr_index_in_I.push_back(i);
          current_data.lr_index_in_R.push_back(j);
        }
      }
    }

    // fill the active energies and make sure, that the energy matrices are 
    // organized with the lset elements being the rows and rset elements
    // being the cols (potentially transpose)
    current_data.lr_energies.resize(lr_edges.size());
    for(uint i = 0; i < lr_edges.size(); ++i) {
      lr_edges[i]->FillActiveEnergies(current_data.lr_energies[i]);
      if(current_data.lset[current_data.lr_index_in_I[i]] > 
         current_data.rset[current_data.lr_index_in_R[i]]) {
        current_data.lr_energies[i].transposeInPlace();
      }
    }

    // Search for all pairs in rset, that have an edge

    std::vector<promod3::core::GMEdge*> rr_edges;
    
    for(int i = 0; i < current_data_rsize; ++i) {
      for(int j = i + 1; j < current_data_rsize; ++j) {

        std::pair<int,int> idx_pair = std::make_pair(current_data.rset[i],
                                                     current_data.rset[j]); 
        std::vector<std::pair<int,int> >::const_iterator idx_it = 
        std::find(edge_indices.begin(), edge_indices.end(), idx_pair);

        if(idx_it != edge_indices.end()) {
          int edge_idx = idx_it - edge_indices.begin();
          rr_edges.push_back(edges[edge_idx]);
          current_data.rr_index_in_R_one.push_back(i);
          current_data.rr_index_in_R_two.push_back(j);
          continue;
        }

        idx_pair = std::make_pair(current_data.rset[j], current_data.rset[i]); 
        idx_it = std::find(edge_indices.begin(), edge_indices.end(), idx_pair);
        if(idx_it != edge_indices.end()) {
          int edge_idx = idx_it - edge_indices.begin();
          rr_edges.push_back(edges[edge_idx]);
          current_data.rr_index_in_R_one.push_back(i);
          current_data.rr_index_in_R_two.push_back(j);
        }
      }
    }

    // fill the active energies and make sure, that the energy matrices are 
    // organized with the rset element with the lower overall index defining
    // the rows and the other the cols
    current_data.rr_energies.resize(rr_edges.size());
    for(uint i = 0; i < rr_edges.size(); ++i) {
      rr_edges[i]->FillActiveEnergies(current_data.rr_energies[i]);
      if(current_data.rset[current_data.rr_index_in_R_one[i]] > 
         current_data.rset[current_data.rr_index_in_R_two[i]]) {
        current_data.rr_energies[i].transposeInPlace();
      }
    }
  }
}


void EnumerateBagData(std::vector<BagData>& bag_data) {

  for(uint bag_idx = 0; bag_idx < bag_data.size(); ++bag_idx) {

    // data of the current bag
    BagData& data = bag_data[bag_idx];
    int rset_size = data.rset.size();
    int num_lr_energies = data.lr_energies.size();
    int num_rr_energies = data.rr_energies.size();

    // the lset of this bag we have to enumerate
    promod3::core::Enumerator<unsigned short> 
    l_set_enumerator(data.num_l_solutions);
    std::vector<unsigned short>& current_l_enum = 
    l_set_enumerator.CurrentEnum();

    int num_children = data.children_indices.size();
    std::vector<int> partial_l_indices(num_children, 0);

    do {      

      partial_l_indices.assign(num_children, 0);
      for(int j = 0; j < num_children; ++j) {
        for(int k = 0; k < data.num_lset_child_l[j]; ++k) {
          partial_l_indices[j] += 
          (current_l_enum[data.lset_child_l_indices[j][k]] *
          data.lset_child_l_index_helpers[j][k]);
        }
      }

      promod3::core::Enumerator<unsigned short> 
      r_set_enumerator(data.num_r_solutions);  
      std::vector<unsigned short>& current_r_enum = 
      r_set_enumerator.CurrentEnum();

      Real score;
      Real best_score = std::numeric_limits<Real>::max();
      std::vector<unsigned short> best_rset = current_r_enum;

      // let's find the optimal r combination given the current l combination... 
      do {      

        score = 0.0;

        // sum up the r self energies
        for(int i = 0; i < rset_size; ++i) {
          score += data.r_self_energies[i][current_r_enum[i]];
        }

        // sum up the lr pairwise energies
        for(int i = 0; i < num_lr_energies; ++i) {
          score += data.lr_energies[i](current_l_enum[data.lr_index_in_I[i]],
                                       current_r_enum[data.lr_index_in_R[i]]);
        }

        // sum up the rr pairwise energies
        for(int i = 0; i < num_rr_energies; ++i) {
          score += data.rr_energies[i](current_r_enum[data.rr_index_in_R_one[i]],
                                       current_r_enum[data.rr_index_in_R_two[i]]);
        }

        for(int i = 0; i < num_children; ++i) {
          int l_idx = partial_l_indices[i];
          for(int j = 0; j < data.num_rset_child_l[i]; ++j) {
            l_idx += (current_r_enum[data.rset_child_l_indices[i][j]] *
                      data.rset_child_l_index_helpers[i][j]);
          }
          score += bag_data[data.children_indices[i]].optimal_r_score[l_idx];
        }

        if(score < best_score) {
          best_score = score;
          best_rset = current_r_enum;
        }

      } while(r_set_enumerator.Next());

      int l_idx = data.lset_idx_calculator.GetLIdx(current_l_enum);
      memcpy(&data.optimal_rset_combination[l_idx*rset_size],
             &best_rset[0], rset_size * sizeof(unsigned short));
      data.optimal_r_score[l_idx] = best_score;

    } while(l_set_enumerator.Next());
  } 
}


void CollectSolutionFromBagData(const std::vector<BagData>& bag_data,
                                int bag_idx, std::vector<int>& solution) {

  const BagData& bag = bag_data[bag_idx];

  // collect the lset values from the solution
  std::vector<unsigned short> l_solution(bag.lset.size(), 0);
  for(uint i = 0; i < l_solution.size(); ++i) {
    l_solution[i] = solution[bag.lset[i]];
  }

  // get the index of the ideal r_combination given that l solution
  int r_idx = bag.lset_idx_calculator.GetLIdx(l_solution);
  // and fill the according values into the solution
  int rset_size = bag.rset.size();
  int data_start_idx = r_idx * rset_size;
  for(int i = 0; i < rset_size; ++i) {
    solution[bag.rset[i]] = bag.optimal_rset_combination[data_start_idx+i];
  }

  // recursively call this function for all the children
  for(std::vector<int>::const_iterator i = bag.children_indices.begin();
      i != bag.children_indices.end(); ++i) {
    CollectSolutionFromBagData(bag_data, *i, solution);
  }
}


/////////////////////////////////////////////////////////////////
//FUNCTIONS AND DATA STRUCTURES FOR THE ASTAR SOLVING ALGORITHM//
/////////////////////////////////////////////////////////////////

struct AStarNode{

  AStarNode(uint idx, uint p_idx, unsigned short s_idx, 
            unsigned short t_d, Real gs, Real hs): node_idx(idx),
                                                   parent_idx(p_idx),
                                                   solution_idx(s_idx),
                                                   tree_depth(t_d),
                                                   gstar(gs),
                                                   hstar(hs) { }

  uint node_idx;
  uint parent_idx;
  unsigned short solution_idx;
  unsigned short tree_depth;
  Real gstar;
  Real hstar;
};


struct CompareAStarNodes{
    bool operator() (AStarNode* a, AStarNode* b) {
        return (a->gstar + a->hstar) > (b->gstar + b->hstar);
    }
};


void ReconstructSearchTreePath(const std::vector<AStarNode>& node_array,
                               uint start_idx, 
                               std::vector<int>& path) {

  path.resize(node_array[start_idx].tree_depth);
  uint current_node_idx = start_idx;

  while(current_node_idx != 0) {
    path[node_array[current_node_idx].tree_depth-1] = 
    node_array[current_node_idx].solution_idx;
    current_node_idx = node_array[current_node_idx].parent_idx;
  }
}


/////////////////////////////////////////////////////////////////
//FUNCTIONS AND DATA STRUCTURES FOR THE MC SOLVING ALGORITHM//
/////////////////////////////////////////////////////////////////

Real GetSolutionEnergy(int node_idx, int solution_idx,
            const std::vector<int>& current_solution,
            const std::vector<std::vector<Real> >& self_energies,
            const std::vector<promod3::core::EMatXX>& pairwise_energies,
            const std::vector<std::vector<std::pair<int,int> > >& 
            pairwise_energy_mapper) {

  Real e = self_energies[node_idx][solution_idx];

  const std::vector<std::pair<int, int> >& e_mapper = 
  pairwise_energy_mapper[node_idx];
  int num_pairwise_energies = e_mapper.size();

  for(int i = 0; i < num_pairwise_energies; ++i) {

    int other_node_idx = e_mapper[i].first;
    int emat_idx = e_mapper[i].second;

    if(other_node_idx > node_idx) {
      e += pairwise_energies[emat_idx](solution_idx, 
                                       current_solution[other_node_idx]);
    }
    else {
      e += pairwise_energies[emat_idx](current_solution[other_node_idx], 
                                       solution_idx); 
    }
  }

  return e;
}


Real GetGlobalEnergy(const std::vector<int>& solution,
               const std::vector<std::vector<Real> >& self_energies,
               const std::vector<promod3::core::EMatXX>& pairwise_energies,
               const std::vector<std::vector<std::pair<int, int> > >&
               pairwise_energy_mapper) {

  Real e = 0.0;

  // sum up self energies
  int num_nodes = solution.size();
  for(int i = 0; i < num_nodes; ++i) {
    e += self_energies[i][solution[i]];
  }

  // sum up pairwise energies
  for(int i = 0; i < num_nodes; ++i) {
    const std::vector<std::pair<int, int> >& e_mapper = 
    pairwise_energy_mapper[i];
    int num_pairwise_energies = e_mapper.size();
    for(int j = 0; j < num_pairwise_energies; ++j) {
      int other_node_idx = e_mapper[j].first;
      if(other_node_idx > i) {
        int emat_idx = e_mapper[j].second;
        e += pairwise_energies[emat_idx](solution[i], solution[other_node_idx]);
      }
    }
  }

  return e;
}


struct SolutionSelector {

  SolutionSelector(std::vector<int> num_solutions, int random_seed): 
                                     n(num_solutions.size()),
                                     n_solutions(num_solutions),
                                     rng(random_seed),
                                     zeroone(0.0, 1.0) { }

  int SelectNode() {
    return std::max(0, std::min(n-1, static_cast<int>(zeroone(rng) * n)));
  }

  int SelectSolution(int node_idx) {
    return std::max(0, std::min(n_solutions[node_idx]-1, static_cast<int>(zeroone(rng) * n_solutions[node_idx])));
  }

  void InitSolutionSelection(std::vector<int>& current_solution) {
    current_solution.resize(n);
    for(int i = 0; i < n; ++i) {
      current_solution[i] = this->SelectSolution(i);
    }
  }

  Real GetRandomNumber() {
    return zeroone(rng);
  }

  int n;
  std::vector<int> n_solutions;
  std::mt19937 rng;
  std::uniform_real_distribution<> zeroone;
};

} // anon ns

namespace promod3{ namespace core{

GMEdge::GMEdge(uint index, 
               GMNode* node_a, 
               GMNode* node_b,  
               const EMatXX& emat): index_(index),
                                    active_(true) {

  if(node_a->GetNumSolutions() != static_cast<uint>(emat.rows())) {
    throw promod3::Error("Number of solutions in node_a must be number "
                         "of rows in emat!");
  }

  if(node_b->GetNumSolutions() != static_cast<uint>(emat.cols())) {
    throw promod3::Error("Number of solutions in node_b must be number "
                         "of cols in emat!");
  }

  if(node_a->GetIndex() == node_b->GetIndex()) {
    throw promod3::Error("Cant Initialize GMEdge with the same GMNode Twice!");
  }

  // node_1 is by definition the one with the lower index
  if(node_a->GetIndex() < node_b->GetIndex()) {
    node1_ = node_a;
    node2_ = node_b;
    emat_ = emat;
  }
  else {
    node1_ = node_b;
    node2_ = node_a;
    emat_ = emat.transpose();
  }
}


void GMEdge::Reset() {
  active_ = true;
}


bool GMEdge::Decompose(Real thresh) {

  if(!active_) return false;

  Real summed_e = 0.0;
  uint m = node1_->GetNumActiveSolutions();
  uint n = node2_->GetNumActiveSolutions();

  for(GMNode::iterator i = node1_->active_solutions_begin();
      i != node1_->active_solutions_end(); ++i) {
    for(GMNode::iterator j = node2_->active_solutions_begin();
        j != node2_->active_solutions_end(); ++j) {
      summed_e += emat_(*i, *j);
    }
  }

  Real avg_value = summed_e/(2*n*m);

  std::vector<Real> ak(m);
  std::vector<Real> bl(n);

  uint m_counter = 0;
  for(GMNode::iterator i = node1_->active_solutions_begin();
      i != node1_->active_solutions_end(); ++i) {
    summed_e = 0.0;
    for(GMNode::iterator j = node2_->active_solutions_begin();
        j != node2_->active_solutions_end(); ++j) {
      summed_e += emat_(*i, *j);
    }
    ak[m_counter] = summed_e/n - avg_value;
    ++m_counter;
  }

  uint n_counter = 0;
  for(GMNode::iterator i = node2_->active_solutions_begin();
      i != node2_->active_solutions_end(); ++i) {
    summed_e = 0.0;
    for(GMNode::iterator j = node1_->active_solutions_begin();
        j != node1_->active_solutions_end(); ++j) {
      summed_e += emat_(*j, *i);
    }
    bl[n_counter] = summed_e/m - avg_value;

    // we directly check, whether any of the energy approximations
    // related to *i is above the thresh and don't evaluate the rest
    // if not necessary
    m_counter = 0;
    for(GMNode::iterator j = node1_->active_solutions_begin();
        j != node1_->active_solutions_end(); ++j) {
      if(std::abs(emat_(*j, *i) - ak[m_counter] - bl[n_counter]) > thresh) { 
        return false;
      }
      ++m_counter;
    }
    ++n_counter;
  }

  // it ran through! This edge can be decomposed!

  int counter = 0;
  for(GMNode::iterator j = node1_->active_solutions_begin(); 
    j != node1_->active_solutions_end(); ++j, ++counter) {
    node1_->AddValue(*j, ak[counter]);
  }

  counter = 0;
  for(GMNode::iterator j = node2_->active_solutions_begin(); 
      j != node2_->active_solutions_end(); ++j, ++counter) {
    node2_->AddValue(*j, bl[counter]);
  }

  this->Deactivate();
  node1_->RemoveFromActiveEdges(this);
  node2_->RemoveFromActiveEdges(this);

  return true;
}


Real GMEdge::EMinDiff(const GMNode* node_ptr, uint idx1, uint idx2) const {

  Real min_e = std::numeric_limits<Real>::max();

  if(node_ptr == node1_) {
    for(GMNode::const_iterator i = node2_->active_solutions_begin();
        i != node2_->active_solutions_end(); ++i) {
      min_e = std::min(min_e, emat_(idx1, *i) - emat_(idx2, *i));
    }
    return min_e;
  }
  else if(node_ptr == node2_) {
    for(GMNode::const_iterator i = node1_->active_solutions_begin();
        i != node1_->active_solutions_end(); ++i) {
      min_e = std::min(min_e, emat_(*i, idx1) - emat_(*i, idx2));
    }
    return min_e;
  }

  throw promod3::Error("Cannot evaluate Node which is unconnected to the edge "
                       "of interest!");
}


void GMEdge::FillActiveEnergies(EMatXX& emat) const {

  int idx1 = 0;
  int idx2 = 0;
  emat = EMatXX::Zero(node1_->GetNumActiveSolutions(), 
                      node2_->GetNumActiveSolutions());

  for(GMNode::const_iterator i = node1_->active_solutions_begin();
      i != node1_->active_solutions_end(); ++i, ++idx1) {
    idx2 = 0;
    for(GMNode::const_iterator j = node2_->active_solutions_begin();
        j != node2_->active_solutions_end(); ++j, ++idx2) {
      emat(idx1, idx2) = emat_(*i, *j);
    }
  }
}


GMNode* GMEdge::GetOtherNode(GMNode* node) {
  if(node == node1_) return node2_;
  if(node == node2_) return node1_;
  throw promod3::Error("Node does not belong to edge!");
}


GMNode::GMNode(uint index, 
               const std::vector<Real>& self_energies): 
                                        index_(index),
                                        active_(true),
                                        self_energies_(self_energies),
                                        original_self_energies_(self_energies) {

  for(uint i = 0; i < self_energies_.size(); ++i) {
    active_solutions_.push_back(i);
  }

  this->FillSortedActiveSolutions();
}


void GMNode::Reset() {

  active_ = true;
  self_energies_ = original_self_energies_;

  // clear everything
  active_solutions_.clear();
  active_edges_.clear();

  // and refill
  for(uint i = 0; i < self_energies_.size(); ++i) {
    active_solutions_.push_back(i);
  }
  this->FillSortedActiveSolutions();
  

  for(uint i = 0; i < edges_.size(); ++i) {
    active_edges_.push_back(i);
  }

  for(std::vector<GMEdge*>::iterator i = edges_.begin(); 
      i != edges_.end(); ++i) { 
    (*i)->Reset();
  }
}


void GMNode::Deactivate(int index) {

  if(!active_) {
    return;
  }

  active_ = false;

  // if the given index is -1 (default), we simply search for the
  // the solution with the lowest self energy as the last active solution,
  // a.k.a. the solution for this node.
  // In all other cases we take the solution at given index.
  if(index == -1) {
    // lets first figure out which is the lowest energy
    // solution and set it to the only active one
    Real e_min = std::numeric_limits<Real>::max();
    index = 0;
    for(uint i = 0; i < active_solutions_.size(); ++i) {
      if(self_energies_[active_solutions_[i]] < e_min) {
        e_min = self_energies_[active_solutions_[i]];
        index = active_solutions_[i];
      }
    }
  }

  active_solutions_.resize(1);
  active_solutions_[0] = index;

  sorted_active_solutions_.resize(1);
  sorted_active_solutions_[0] = index;
  
  // deactivate all edges connected to this node
  for(iterator i = this->active_edges_begin(); 
      i != this->active_edges_end(); ++i) {
    GMNode* other = edges_[*i]->GetOtherNode(this);
    other->RemoveFromActiveEdges(edges_[*i]);
    edges_[*i]->Deactivate();
  } 
  active_edges_.clear();
}


void GMNode::AddEdge(GMEdge* edge) {
  active_edges_.push_back(edges_.size());
  edges_.push_back(edge);
}


void GMNode::AddValue(uint node_idx, Real val) {
  self_energies_[node_idx] += val;
}


void GMNode::RemoveFromActiveEdges(GMEdge* edge) {

  for(uint i = 0; i < edges_.size(); ++i) {
    if(edge == edges_[i]) {
      iterator it = std::find(active_edges_.begin(), active_edges_.end(), i);
      if(it != active_edges_.end()) active_edges_.erase(it);
    }
  }
}


bool GMNode::DEE(Real e_cut) {

  if(active_solutions_.size() <= 1) return false;

  bool something_happened = false; 
  Real sum;
  int solution_to_eliminate = sorted_active_solutions_.size()-1;

  while(solution_to_eliminate >= 0) {

    for(int i = 0; i < static_cast<int>(sorted_active_solutions_.size()); ++i) {

      if(i == solution_to_eliminate) continue;

      sum = 
      self_energies_[sorted_active_solutions_[solution_to_eliminate]] -
      self_energies_[sorted_active_solutions_[i]];

      for(iterator j = this->active_edges_.begin(); 
          j != this->active_edges_.end(); ++j) {
        sum += 
        edges_[*j]->EMinDiff(this,
                    sorted_active_solutions_[solution_to_eliminate],
                    sorted_active_solutions_[i]);
      }

      if(sum >= e_cut) {
        
        something_happened = true;
        std::vector<uint>::iterator it = 
        std::find(active_solutions_.begin(), active_solutions_.end(),
                  sorted_active_solutions_[solution_to_eliminate]);
        active_solutions_.erase(it);
        sorted_active_solutions_.erase(sorted_active_solutions_.begin() + 
                                       solution_to_eliminate);
        break;
      }
    }
    --solution_to_eliminate;
  }

  return something_happened;
}


void GMNode::FillActiveSelfEnergies(std::vector<Real>& self_energies) const {

  self_energies.resize(active_solutions_.size());

  for(uint i = 0; i < active_solutions_.size(); ++i) {
    self_energies[i] = self_energies_[active_solutions_[i]];
  }
}


void GMNode::FillSortedActiveSolutions() {

  if(active_solutions_.size() != self_energies_.size()) {
    throw promod3::Error("Can only sort active solutions when ALL solutions "
                         "are active!");
  }

  std::vector<std::pair<Real, uint> > temp;
  for(uint i = 0; i < self_energies_.size(); ++i) {
    temp.push_back(std::make_pair(self_energies_[i], i));
  }
  std::sort(temp.begin(), temp.end());

  sorted_active_solutions_.clear();
  for(uint i = 0; i < temp.size(); ++i) {
    sorted_active_solutions_.push_back(temp[i].second);
  }
}


GraphMinimizer::~GraphMinimizer() {

  for(uint i = 0; i < nodes_.size(); ++i) {
    delete nodes_[i];
  }

  for(uint i = 0; i < edges_.size(); ++i) {
    delete edges_[i];
  }  
}


int GraphMinimizer::AddNode(const std::vector<Real>& self_energies) {

  int node_idx = nodes_.size();
  nodes_.push_back(new GMNode(node_idx, self_energies));
  return node_idx;
}


int GraphMinimizer::AddEdge(uint node_idx_one, uint node_idx_two,
                            const EMatXX& emat) {

  if(node_idx_one >= nodes_.size() || node_idx_two >= nodes_.size()) {
    throw promod3::Error("Invalid node idx provided!");
  }

  int edge_idx = edges_.size();
  // check for valid energy matrix happens in GMEdge constructor
  edges_.push_back(new GMEdge(edge_idx, nodes_[node_idx_one],
                              nodes_[node_idx_two], emat));

  // the edge also has to be added to the nodes
  nodes_[node_idx_one]->AddEdge(edges_.back());
  nodes_[node_idx_two]->AddEdge(edges_.back());

  return edge_idx;
}


bool GraphMinimizer::ApplyDEE(uint idx, Real e_thresh) {

  if(idx >= nodes_.size()) {
    throw promod3::Error("Invalid Index observed when applying DEE!");
  }

  return nodes_[idx]->DEE(e_thresh);
}


bool GraphMinimizer::ApplyEdgeDecomposition(uint idx, Real e_thresh) {

  if(idx >= edges_.size()) {
    throw promod3::Error("Invalid Index observed when applying Edge "
                         "Decomposition!");
  }

  return edges_[idx]->Decompose(e_thresh);
}


void GraphMinimizer::Prune(Real epsilon, Real e_cut, bool consider_all_nodes) {

  // nodes/edges, where something happened
  std::vector<bool> hot_edges(edges_.size(),true);
  std::vector<bool> hot_nodes;
  if(consider_all_nodes)hot_nodes.resize(nodes_.size(),true);   
  else hot_nodes.resize(nodes_.size(),false);

  bool something_happened = true;
  while(something_happened) {
    something_happened = false;
    // let's first do edge decomposition
    for(uint i = 0; i < hot_edges.size(); ++i) {
      if(!hot_edges[i]) continue;
      if(this->ApplyEdgeDecomposition(i, epsilon)) {
        // let's set the status hot to the nodes connected by the
        // edge
        hot_nodes[edges_[i]->GetNode1()->GetIndex()] = true;
        hot_nodes[edges_[i]->GetNode2()->GetIndex()] = true;
        something_happened = true;
      }
      // either nothing happened, or successfully decomposed...
      // edge is not hot anymore
      hot_edges[i] = false;
    }
    // let's do DEE
    for(uint i = 0; i < hot_nodes.size(); ++i) {
      if(!hot_nodes[i]) continue;
      if(this->ApplyDEE(i, e_cut)) {
        // let's set the status hot to the edges, as well as other
        // nodes connected to that node
        for(GMNode::iterator j = nodes_[i]->active_edges_begin();
            j != nodes_[i]->active_edges_end(); ++j) {
          GMEdge* current_edge = nodes_[i]->GetEdge(*j);
          hot_edges[current_edge->GetIndex()] = true;
          // one of the following connected node is the checked node
          // itself... will be set to false again...
          hot_nodes[current_edge->GetNode1()->GetIndex()] = true;
          hot_nodes[current_edge->GetNode2()->GetIndex()] = true;
        }
        something_happened = true;
      }
      // either nothing happened, or successful DEE...
      // node is not hot anymore
      hot_nodes[i] = false;
    }
  }
}


void GraphMinimizer::Reset() {

  for(std::vector<GMNode*>::iterator i = nodes_.begin(); 
      i != nodes_.end(); ++i) {
    (*i)->Reset();
  }
  for(std::vector<GMEdge*>::iterator i = edges_.begin(); 
      i != edges_.end(); ++i) {
    (*i)->Reset();
  }
}


promod3::core::Graph GraphMinimizer::ToRawGraph() const {

  promod3::core::Graph raw_graph(nodes_.size());
  std::map<GMNode*, int> ptr_mapper;
  for(uint i = 0; i < nodes_.size(); ++i) {
    ptr_mapper[nodes_[i]] = i;
  }
  for(std::vector<GMEdge*>::const_iterator i = edges_.begin(); 
      i != edges_.end(); ++i) {
    if((*i)->IsActive()) {
      int idx_one = ptr_mapper[(*i)->GetNode1()];
      int idx_two = ptr_mapper[(*i)->GetNode2()];
      raw_graph.Connect(idx_one,idx_two);
    }
  }
  return raw_graph;
}


bool GraphMinimizer::HasActiveNodes() const {

  for(uint i = 0; i < nodes_.size(); ++i) {
    if(nodes_[i]->IsActive()) {
      return true;
    }
  }

  return false;
}


std::pair<std::vector<int>, Real> 
GraphMinimizer::TreeSolve(uint64_t max_complexity, Real initial_epsilon) {

  core::ScopedTimerPtr prof = core::StaticRuntimeProfiler::StartScoped(
                                "GraphMinimizer::TreeSolve", 2);

  // all nodes must be active in the beginning
  for(std::vector<GMNode*>::iterator i = nodes_.begin(); 
      i != nodes_.end(); ++i) {
    if(!(*i)->IsActive()) {
      String err = "All nodes of graph must be active for TreeSolving.";
      err += " Either construct a new graph or reset the current one!";
      throw promod3::Error(err);
    }
  }

  std::vector<int> overall_solution(nodes_.size(), 0);
  Real overall_energy = 0.0;

  // handle the case where graph is empty
  if(nodes_.empty()) {
    return std::make_pair(overall_solution, overall_energy);
  }

  // handle the case where graph only contains one item
  if(nodes_.size() == 1) {
    nodes_[0]->Deactivate();
    overall_solution[0] = nodes_[0]->GetOverallIndex(0); 
    overall_energy += nodes_[0]->GetSelfEnergy(overall_solution[0]);
    return std::make_pair(overall_solution, overall_energy);
  }

  bool first_iteration = true;
  
  while(this->HasActiveNodes()) {

    this->Prune(initial_epsilon, 0.0, first_iteration);

    // build raw graph and extract connected components
    promod3::core::Graph raw_graph = this->ToRawGraph();

    // generate the connected components
    std::vector<std::vector<int> > connected_components;
    raw_graph.ConnectedComponents(connected_components);
    
    // solve the connected components seperately if the according
    // complexity is feasible

    for(uint component_idx = 0; component_idx < connected_components.size();
        ++component_idx) {

      const std::vector<int>& component = connected_components[component_idx];

      if(component.size() == 1) {
        if(nodes_[component[0]]->IsActive()) {
          // note, that lowest energy solution will be the only active one upon
          // calling Deactivate
          nodes_[component[0]]->Deactivate();
          int overall_idx = nodes_[component[0]]->GetOverallIndex(0);          
          overall_solution[component[0]] = overall_idx;
          overall_energy += nodes_[component[0]]->GetSelfEnergy(overall_idx);
        }
        continue;
      }

      // create initial data required to solve the enumeration problem
      std::vector<int> num_active_solutions(component.size());
      for(uint i = 0; i < component.size(); ++i) {
        // Check, whether any of the nodes has too many solutions
        // (due to memory reasons we use unsigned shorts as solution 
        //  identifier in the tree solving algorithms)
        if(nodes_[component[i]]->GetNumActiveSolutions() > 
           std::numeric_limits<unsigned short>::max()) {
          std::stringstream ss;
          ss << "Due to technical reasons the maximum number of solutions per ";
          ss << "position is limited to ";
          ss << std::numeric_limits<unsigned short>::max();
          ss << ". Observed a location with ";
          ss << nodes_[component[i]]->GetNumActiveSolutions();
          ss << " solutions. Either use less solutions or perform more pruning";
          ss << " operations before using the TreeSolve algorithm.";
          throw promod3::Error(ss.str());
        }
        num_active_solutions[i] = nodes_[component[i]]->GetNumActiveSolutions(); 
      }

      promod3::core::Graph component_graph = raw_graph.SubGraph(component);

      TBag* component_tree = 
      promod3::core::GenerateMinimalWidthTree(component_graph);

      // generate post order traversal of previously constructed tree
      std::vector<TBag*> traversal = 
      promod3::core::PostOrderTraversal(component_tree);

      // this only fills some initial data, that is required to estimate
      // the complexity to solve this component
      std::vector<BagData> bag_data;
      uint64_t component_complexity = FillRawBagData(traversal,
                                                     num_active_solutions,
                                                     bag_data);

      // check whether complexity of current connected component is solvable...
      // if not, we simply perform another iteration of pruning.
      if(component_complexity > max_complexity) {
        if(initial_epsilon <= 0.0) {
          std::stringstream ss;
          ss << "One of the connected components in the GraphMinimizer ";
          ss << "reaches a complexity of "<< component_complexity;
          ss << ", which is above the max_complexity of "<<max_complexity;
          ss << ". The idea is to prune more and more aggressively by ";
          ss << "multiplying the initial_epsilon value by a factor of 2. ";
          ss << "Your initial_epsilon is "<<initial_epsilon;
          ss << ", the complexity of this connected ";
          ss << "component will therefore never fall below max_complexity. ";
          ss << "To successfully solve the Graph you either have to ";
          ss << "give initial_epsilon a value above 0.0 or increase ";
          ss << "max_complexity";
          throw promod3::Error(ss.str());
        }
        continue;
      }

      // The complexity is low enough... let's fill remaining data and
      // solve it! GOGOGO!

      std::vector<GMNode*> component_nodes;
      std::vector<GMEdge*> component_edges;
      std::vector<std::pair<int, int> > component_edge_indices;

      for(uint i = 0; i < component.size(); ++i) {
        component_nodes.push_back(nodes_[component[i]]);
      }

      for(uint i = 0; i < edges_.size(); ++i) {
        
        GMEdge* edge = edges_[i];

        // is the edge active?
        if(!edge->IsActive()) continue;

        // does it affect the current component?
        int a = edge->GetNode1()->GetIndex();
        int b = edge->GetNode2()->GetIndex();

        std::vector<int>::const_iterator a_it = std::find(component.begin(), 
                                                          component.end(), a);
        if(a_it == component.end()) continue; // it's not part of the component

        std::vector<int>::const_iterator b_it = std::find(component.begin(), 
                                                          component.end(), b);
        if(b_it == component.end()) continue; // it's not part of the component

        int a_in_component = a_it - component.begin();
        int b_in_component = b_it - component.begin();

        component_edges.push_back(edge);
        component_edge_indices.push_back(std::make_pair(a_in_component,
                                                        b_in_component));
      }

      // let's fill in remaining data (energies and stuff)
      FillBagData(traversal, component_nodes, component_edges, 
                  component_edge_indices, bag_data);      

      // let's do the whole enumeration game
      EnumerateBagData(bag_data);

      // let's get the solution out
      std::vector<int> component_solution(component.size(), 0);
      CollectSolutionFromBagData(bag_data, bag_data.size() - 1, 
                                 component_solution);

      overall_energy += bag_data.back().optimal_r_score[0];

      // alter the graph accordingly
      for(uint i = 0; i < component.size(); ++i) {
        uint overall_idx = 
        nodes_[component[i]]->GetOverallIndex(component_solution[i]);
        nodes_[component[i]]->Deactivate(overall_idx);
        overall_solution[component[i]] = overall_idx;
      }
    }

    first_iteration = false;
    initial_epsilon *= 2;
  }

  return std::make_pair(overall_solution, overall_energy);
}


std::pair<std::vector<std::vector<int> >, std::vector<Real> > 
GraphMinimizer::AStarSolve(Real e_thresh, uint max_n, uint max_visited_nodes) {

  core::ScopedTimerPtr prof = core::StaticRuntimeProfiler::StartScoped(
                                "GraphMinimizer::AStarSolve", 2);

  // all nodes must be active in the beginning
  for(std::vector<GMNode*>::iterator i = nodes_.begin();
      i != nodes_.end(); ++i) {
    if(!(*i)->IsActive()) {
      String err = "All nodes of graph must be active for AStarSolving.";
      err += " Either construct a new graph or reset the current one!";
      throw promod3::Error(err);
    }
  }

  // e_thresh and max_n must be positive
  if(e_thresh < 0.0) throw promod3::Error("e_thresh must be positive!");
  if(max_n <= 0) throw promod3::Error("max_n must be positive!");
  
  std::vector<std::vector<int> > solutions;
  std::vector<Real> solution_energies;
  int num_graph_nodes = nodes_.size();
  int num_graph_edges = edges_.size();

  // handle the case where graph is empty
  if(num_graph_nodes == 0) {
    return std::make_pair(solutions, solution_energies);
  }

  // handle the case where graph only contains one item
  if(num_graph_nodes == 1) {

    uint num_active_solutions = nodes_[0]->GetNumActiveSolutions();
    if(num_active_solutions == 0) {
      return std::make_pair(solutions, solution_energies);
    }

    std::vector<std::pair<Real, int> > sorted_solutions;
    sorted_solutions.reserve(num_active_solutions);
    std::vector<Real> active_energies;
    nodes_[0]->FillActiveSelfEnergies(active_energies);
    for(uint i = 0; i < num_active_solutions; ++i) {
      sorted_solutions.push_back(std::make_pair(active_energies[i], i));
    }
    std::sort(sorted_solutions.begin(), sorted_solutions.end());
    Real emin = sorted_solutions[0].first;
    for(uint i = 0; i < std::min(max_n, num_active_solutions); ++i) {
      if((sorted_solutions[i].first - emin) > e_thresh || 
         solutions.size() >= max_n) {
        break;
      }
      int overall_idx = nodes_[0]->GetOverallIndex(sorted_solutions[i].second);
      solutions.push_back(std::vector<int>(1, overall_idx));
      solution_energies.push_back(sorted_solutions[i].first);
    }
    return std::make_pair(solutions, solution_energies);
  }

  // extract the energies of the active solutions
  std::vector<int> num_active_solutions(nodes_.size());
  std::vector<std::vector<Real> > self_energies(nodes_.size());

  for(int i = 0; i < num_graph_nodes; ++i) {
    num_active_solutions[i] = nodes_[i]->GetNumActiveSolutions();
  }

  for(int i = 0; i < num_graph_nodes; ++i) {
    nodes_[i]->FillActiveSelfEnergies(self_energies[i]);
  }

  // pairwise energies are stored a bit more complicated...
  // One vector per node.
  // Every element in that vector contains the index to another node with LOWER
  // idx and the corresponding energy matrix
  std::vector<std::vector<std::pair<int, EMatXX> > > 
  pairwise_energies(num_graph_nodes);
  std::vector<int> num_pairwise_energies(num_graph_nodes);

  // to avoid too many reallocations and copying of the eigen matrices, we
  // build up a temporary data structure first  
  std::vector<std::vector<std::pair<int, GMEdge*> > > temp(num_graph_nodes);

  for(int i = 0; i < num_graph_edges; ++i) {
    if(edges_[i]->IsActive()) {
      GMEdge* edge = edges_[i];
      int a = edge->GetNode1()->GetIndex();
      int b = edge->GetNode2()->GetIndex();

      // b is larger...
      temp[b].push_back(std::make_pair(a, edge));
    }
  } 

  for(int i = 0; i < num_graph_nodes; ++i) {
    int n = temp[i].size();
    num_pairwise_energies[i] = n;
    pairwise_energies[i].resize(n);
    for(int j = 0; j < n; ++j) {
      pairwise_energies[i][j].first = temp[i][j].first;
      temp[i][j].second->FillActiveEnergies(pairwise_energies[i][j].second);
    }
  }

  // following data structure is a precalculation to increase the speed
  // of hstar calculation...

  // for every every position there is a vector for every solution at 
  // that position. Every element in that vector contains the minimal 
  // energy towards every position that comes before, zero if no 
  // interaction is observed
  std::vector<std::vector<std::vector<Real> > > 
  min_pairwise_e_before(num_graph_nodes, std::vector<std::vector<Real> >());

  for(int i = 0; i < num_graph_nodes; ++i) {
    min_pairwise_e_before[i].resize(num_active_solutions[i]);
    for(int j = 0; j < num_active_solutions[i]; ++j) {
      min_pairwise_e_before[i][j].assign(i,0.0);
      for(int k = 0; k < num_pairwise_energies[i]; ++k) {

        int other_node = pairwise_energies[i][k].first;
        const EMatXX& emat = pairwise_energies[i][k].second; 
        
        Real e_min = std::numeric_limits<Real>::max();
        for(int l = 0; l < num_active_solutions[other_node]; ++l) {
          e_min = std::min(e_min, emat(l, j));
        }

        min_pairwise_e_before[i][j][other_node] = e_min;
      }
    }
  }

  // Data structures to hold the visited nodes and sort them in a priority queue
  std::priority_queue<AStarNode*,std::vector<AStarNode*>,CompareAStarNodes> 
  node_queue;
  std::vector<AStarNode> node_array;
  // we already have to reserve node_array, since we deal with pointers
  // to it and reallocations are a bit bad in these cases...
  node_array.reserve(max_visited_nodes);

  // we first add the root node with g* = 0.0 and h* = max_real
  node_array.push_back(AStarNode(0, 0, 0, 0, 0.0, 
                                 std::numeric_limits<Real>::max()));

  node_queue.push(&node_array[0]);
  Real min_energy = std::numeric_limits<Real>::max();

  while(!node_queue.empty()) {

    AStarNode* current_node = node_queue.top();
    node_queue.pop();

    std::vector<int> path;
    ReconstructSearchTreePath(node_array, current_node->node_idx, path);

    if(current_node->tree_depth >= num_graph_nodes) {
      // it is a solution!!!!!
      Real fstar = current_node->gstar; //note, that hstar must be zero...
      if(solutions.empty()) {
        // it's the first solution and therefore the GMEC
        // (Global Minimum Energy Conformation Fuck Yeah...)
        min_energy = fstar;
      }
      else{
        // there is at least one solution around! let's check whether
        // the current solution is within e_thresh
        if(fstar - min_energy > e_thresh) break;
      }

      // let's add it
      solutions.push_back(path);
      solution_energies.push_back(fstar);

      // check whether we have enough solutions
      if(solutions.size() >= max_n) break; 

      continue;
    }

    // current node has to be expanded 
    // => new node for every single solution in node

    // we first check whether we become bigger than max_visited_nodes
    if(node_array.size() + num_active_solutions[current_node->tree_depth] > 
       max_visited_nodes) {
      throw promod3::Error("Reached max_visited_nodes => increase this"
                           "parameter or reduce the problem size!");
    }

    // following variables will be the same for all expaned nodes
    int pos_in_graph = current_node->tree_depth;
    uint parent_idx = current_node->node_idx;
    unsigned short tree_depth = current_node->tree_depth + 1;

    // We already add an element to the path for the node to be expaned.
    // This saves an if in the hstar calculation
    path.push_back(0);

    for(int solution_idx = 0; solution_idx < num_active_solutions[pos_in_graph];
        ++solution_idx) {

      // set the hacky last element of the path
      path.back() = solution_idx;

      // variable specific for expanded node
      uint node_idx = node_array.size();
      Real gstar = current_node->gstar;
      Real hstar = 0.0;

      // we still need to adapt the energy values (gstar and hstar)

      // in case of gstar we have to add the self energy of the current
      // solution and the pairwise energy towards all previous solutions in
      // the path towards this node

      // let's add the self energy
      gstar += self_energies[pos_in_graph][solution_idx];

      // let's add all pairwise energies towards the already set solutions
      for(int i = 0; i < num_pairwise_energies[pos_in_graph]; ++i) {
        int other_node = pairwise_energies[pos_in_graph][i].first;
        const EMatXX& emat = pairwise_energies[pos_in_graph][i].second;
        gstar += emat(path[other_node], solution_idx);
      }

      // in case of hstar we have to add the minimal pairwise energy towards
      // all nodes still to be determined
      int last_path_idx = path.size()-1;
      for(int non_set_pos_idx = pos_in_graph + 1; 
          non_set_pos_idx < num_graph_nodes; ++non_set_pos_idx) {

        Real min_e = std::numeric_limits<Real>::max();

        // iterate over all possible solutions at position non_set_pos_idx
        for(int i = 0; i < num_active_solutions[non_set_pos_idx]; ++i) {

          // start with its self energy
          Real actual_e = self_energies[non_set_pos_idx][i];

          // add all pairwise energies towards solutions that have already been
          // set in this path, the according minimal energy that could be 
          // achieved respectively
          for(int j = 0; j < num_pairwise_energies[non_set_pos_idx]; ++j) {
            int other_node = pairwise_energies[non_set_pos_idx][j].first;
            if(other_node <= last_path_idx) {
              // add the energy to the already determined energy from the path
              const EMatXX& emat = pairwise_energies[non_set_pos_idx][j].second;
              actual_e += emat(path[other_node], i);
            }
            else{
              // add the minimal possible energy...
              actual_e += min_pairwise_e_before[non_set_pos_idx][i][other_node];
            }
          }
          min_e = std::min(min_e, actual_e);
        }
        hstar += min_e;
      }

      node_array.push_back(AStarNode(node_idx, parent_idx,
                                     solution_idx, tree_depth, gstar,
                                     hstar));

      // it's all done, we finally have to insert it into the priority queue
      node_queue.push(&node_array[node_idx]);
    }
  }

  // the solutions are relative to the active solutions, we still have to 
  // translate them into overall indices
  for(uint i = 0; i < solutions.size(); ++i) {
    for(uint j = 0; j < solutions[i].size(); ++j) {
      solutions[i][j] = nodes_[j]->GetOverallIndex(solutions[i][j]);
    }
  } 

  return std::make_pair(solutions, solution_energies);
}


std::pair<std::vector<std::vector<int> >, std::vector<Real> > 
GraphMinimizer::MCSolve(int n, int mc_steps, Real start_temperature, 
                        int change_frequency, Real cooling_factor, int seed) {

  core::ScopedTimerPtr prof = core::StaticRuntimeProfiler::StartScoped(
                                "GraphMinimizer::MCSolve", 2);

  // all nodes must be active in the beginning
  for(std::vector<GMNode*>::iterator i = nodes_.begin();
      i != nodes_.end(); ++i) {
    if(!(*i)->IsActive()) {
      String err = "All nodes of graph must be active for MCSolving.";
      err += " Either construct a new graph or reset the current one!";
      throw promod3::Error(err);
    }
  }

  std::vector<std::vector<int> > solutions;
  std::vector<Real> solution_energies;
  int num_graph_nodes = nodes_.size();
  int num_graph_edges = edges_.size();

  // handle the case where graph is empty
  if(num_graph_nodes == 0) {
    throw promod3::Error("Cannot run MCSolve on an empty Graph!");
  }

  // extract the number of active solutions and the self energies 
  // of all the nodes
  std::vector<int> num_active_solutions(num_graph_nodes);
  std::vector<std::vector<Real> > self_energies(num_graph_nodes);
  for(int i = 0; i < num_graph_nodes; ++i) {
    nodes_[i]->FillActiveSelfEnergies(self_energies[i]);
    num_active_solutions[i] = self_energies[i].size();
  }

  // a vector for every node... every vector contains pairs describing the
  // interactions towards other nodes. 
  // first pair element: idx to OTHER node
  // second pair element: idx to energy mat
  std::vector<std::vector<std::pair<int, int> > > pairwise_energy_mapper;
  pairwise_energy_mapper.resize(num_graph_nodes);
  std::vector<GMEdge*> active_edges;

  // buildup the pairwise_energy_mapper
  for(int i = 0; i < num_graph_edges; ++i) {
    GMEdge* edge = edges_[i];
    if(edge->IsActive()) {
      int n1_idx = edge->GetNode1()->GetIndex();
      int n2_idx = edge->GetNode2()->GetIndex();
      int edge_idx = active_edges.size();
      pairwise_energy_mapper[n1_idx].push_back(std::make_pair(n2_idx, edge_idx));
      pairwise_energy_mapper[n2_idx].push_back(std::make_pair(n1_idx, edge_idx));
      active_edges.push_back(edge);
    }  
  }

  // fill the actual energies
  int num_active_edges = active_edges.size();
  std::vector<EMatXX> pairwise_energies(num_active_edges);
  for(int i = 0; i < num_active_edges; ++i) {
    active_edges[i]->FillActiveEnergies(pairwise_energies[i]);
  } 

  // temporary object to perform a sorting by energy in the end
  std::vector<std::pair<Real, int> > sorting_vector;

  // let the sampling begin!
  SolutionSelector solution_selector(num_active_solutions, seed);

  for(int trajectory_idx = 0; trajectory_idx < n; ++trajectory_idx) {
  
    std::vector<int> current_solution;
    solution_selector.InitSolutionSelection(current_solution);
    Real temperature = start_temperature / cooling_factor;

    for(int current_step = 0; current_step < mc_steps; ++current_step) {

      if (current_step % change_frequency == 0) {
        temperature *= cooling_factor;
      }

      int node_idx = solution_selector.SelectNode();
      int solution_idx = current_solution[node_idx];
      int proposed_solution_idx = solution_selector.SelectSolution(node_idx);

      Real e_current = GetSolutionEnergy(node_idx, solution_idx, 
                                         current_solution, self_energies, 
                                         pairwise_energies, 
                                         pairwise_energy_mapper);

      Real e_new = GetSolutionEnergy(node_idx, proposed_solution_idx, 
                                     current_solution, self_energies, 
                                     pairwise_energies, pairwise_energy_mapper);

      Real e_diff = e_new - e_current;

      if (e_diff < Real(0.0)) {
        current_solution[node_idx] = proposed_solution_idx;
      }
      else {
        Real acceptance_probability = std::exp(-e_diff/temperature);
        if (solution_selector.GetRandomNumber() < acceptance_probability) {
          current_solution[node_idx] = proposed_solution_idx;
        }
      }
    }

    // the trajectory solution!!!
    Real e = GetGlobalEnergy(current_solution, self_energies, pairwise_energies,
                             pairwise_energy_mapper);
    solution_energies.push_back(e); 
    solutions.push_back(current_solution);
    sorting_vector.push_back(std::make_pair(e, trajectory_idx));
  }
  
  // the solutions are relative to the active solutions, we still have to 
  // translate them into overall indices
  for(int i = 0; i < n; ++i) {
    for(int j = 0; j < num_graph_nodes; ++j) {
      solutions[i][j] = nodes_[j]->GetOverallIndex(solutions[i][j]);
    }
  }

  // perform a final sorting by energy
  std::sort(sorting_vector.begin(), sorting_vector.end());
  std::vector<std::vector<int> > sorted_solutions(n);
  std::vector<Real> sorted_solution_energies(n);
  for(int i = 0; i < n; ++i) {
    sorted_solutions[i] = solutions[sorting_vector[i].second];
    sorted_solution_energies[i] = solution_energies[sorting_vector[i].second];
  }

  return std::make_pair(sorted_solutions, sorted_solution_energies);
}


std::pair<std::vector<int>, Real> GraphMinimizer::NaiveSolve() {

  core::ScopedTimerPtr prof = core::StaticRuntimeProfiler::StartScoped(
                                "GraphMinimizer::NaiveSolve", 2);

  // all nodes must be active in the beginning
  for(std::vector<GMNode*>::iterator i = nodes_.begin();
      i != nodes_.end(); ++i) {
    if(!(*i)->IsActive()) {
      String err = "All nodes of graph must be active for NaiveSolving.";
      err += " Either construct a new graph or reset the current one!";
      throw promod3::Error(err);
    }
  }

  int num_graph_nodes = nodes_.size();
  int num_graph_edges = edges_.size();

  // handle the case where graph is empty
  if(num_graph_nodes == 0) {
    throw promod3::Error("Cannot run NaiveSolve on an empty Graph!");
  }

  // extract the number of active solutions and the self energies 
  // of all the nodes
  std::vector<unsigned short> num_active_solutions(num_graph_nodes);
  std::vector<std::vector<Real> > self_energies(num_graph_nodes);
  for(int i = 0; i < num_graph_nodes; ++i) {
    nodes_[i]->FillActiveSelfEnergies(self_energies[i]);
    int active_solutions = self_energies[i].size();
    if(active_solutions > std::numeric_limits<unsigned short>::max()) {
      std::stringstream ss;
      ss << "Due to technical reasons there must be no nodes with more than ";
      ss << active_solutions << " possible solutions in NaiveSolve!";
      throw promod3::Error(ss.str());
    }
    num_active_solutions[i] = active_solutions;
  }

  std::vector<EMatXX> pairwise_energies;
  std::vector<GMEdge*> active_edges;
  std::vector<std::pair<int,int> > pairwise_energy_indices;

  for(int i = 0; i < num_graph_edges; ++i) {
    GMEdge* edge = edges_[i];
    if(edge->IsActive()) {
      int n1_idx = edge->GetNode1()->GetIndex();
      int n2_idx = edge->GetNode2()->GetIndex();
      pairwise_energy_indices.push_back(std::make_pair(n1_idx, n2_idx));
      active_edges.push_back(edge);
    }
  }

  int num_pairwise_energies = active_edges.size(); 
  pairwise_energies.resize(num_pairwise_energies);
  for(int i = 0; i < num_pairwise_energies; ++i) {
    active_edges[i]->FillActiveEnergies(pairwise_energies[i]);
  }

  // This is the most stupid piece of code I ever wrote... SLOOOOOOOW
  promod3::core::Enumerator<unsigned short> enumerator(num_active_solutions);
  std::vector<unsigned short>& current_solution = enumerator.CurrentEnum();

  Real best_energy = std::numeric_limits<Real>::max();
  std::vector<unsigned short> best_solution = current_solution;

  do {

    Real current_energy = 0.0;

    // sum up the internal energies
    for(int i = 0; i < num_graph_nodes; ++i) {
      current_energy += self_energies[i][current_solution[i]];
    }

    for(int i = 0; i < num_pairwise_energies; ++i) {
      int n1_idx = pairwise_energy_indices[i].first;
      int n2_idx = pairwise_energy_indices[i].second;
      current_energy += pairwise_energies[i](current_solution[n1_idx], 
                                             current_solution[n2_idx]);
    }

    if(current_energy < best_energy) {
      best_energy = current_energy;
      best_solution = current_solution;
    }

  } while(enumerator.Next());

  // the solution is relative to the active solution, we still have to 
  // translate them into overall indices
  std::vector<int> final_solution(num_graph_nodes);
  for(int i = 0; i < num_graph_nodes; ++i) {
    final_solution[i] = nodes_[i]->GetOverallIndex(best_solution[i]);
  }

  return std::make_pair(final_solution, best_energy);
}

}} // ns
