diff --git a/src/algorithms.hpp b/src/algorithms.hpp
index 83eeaf8dd7e32f776384d0c47fec5e5d0b43d1a8..40b6e9cc79afbd6dacec475e5d52fd622ae3eed0 100644
--- a/src/algorithms.hpp
+++ b/src/algorithms.hpp
@@ -19,230 +19,301 @@
 
 namespace nest {
 namespace mc {
-namespace algorithms{
-
-    template <typename C>
-    typename C::value_type
-    sum(C const& c)
-    {
-        using value_type = typename C::value_type;
-        return std::accumulate(c.begin(), c.end(), value_type{0});
-    }
-
-    template <typename C>
-    typename C::value_type
-    mean(C const& c)
-    {
-        return sum(c)/c.size();
+namespace algorithms {
+
+template <typename C>
+typename C::value_type
+sum(C const& c)
+{
+    using value_type = typename C::value_type;
+    return std::accumulate(c.begin(), c.end(), value_type{0});
+}
+
+template <typename C>
+typename C::value_type
+mean(C const& c)
+{
+    return sum(c)/c.size();
+}
+
+template <typename C>
+C make_index(C const& c)
+{
+    static_assert(
+        std::is_integral<typename C::value_type>::value,
+        "make_index only applies to integral types"
+    );
+
+    C out(c.size()+1);
+    out[0] = 0;
+    std::partial_sum(c.begin(), c.end(), out.begin()+1);
+    return out;
+}
+
+/// works like std::is_sorted(), but with stronger condition that succesive
+/// elements must be greater than those before them
+template <typename C>
+bool is_strictly_monotonic_increasing(C const& c)
+{
+    using value_type = typename C::value_type;
+    return std::is_sorted(
+        c.begin(),
+        c.end(),
+        [] (value_type const& lhs, value_type const& rhs) {
+            return lhs <= rhs;
+        }
+    );
+}
+
+template <typename C>
+bool is_strictly_monotonic_decreasing(C const& c)
+{
+    using value_type = typename C::value_type;
+    return std::is_sorted(
+        c.begin(),
+        c.end(),
+        [] (value_type const& lhs, value_type const& rhs) {
+            return lhs >= rhs;
+        }
+    );
+}
+
+template <
+    typename C,
+    typename = typename std::enable_if<std::is_integral<typename C::value_type>::value>
+>
+bool is_minimal_degree(C const& c)
+{
+    static_assert(
+        std::is_integral<typename C::value_type>::value,
+        "is_minimal_degree only applies to integral types"
+    );
+
+    if(c.size()==0u) {
+        return true;
     }
 
-    template <typename C>
-    C make_index(C const& c)
-    {
-        static_assert(
-            std::is_integral<typename C::value_type>::value,
-            "make_index only applies to integral types"
-        );
-
-        C out(c.size()+1);
-        out[0] = 0;
-        std::partial_sum(c.begin(), c.end(), out.begin()+1);
-        return out;
+    using value_type = typename C::value_type;
+    if(c[0] != value_type(0)) {
+        return false;
     }
-
-    /// works like std::is_sorted(), but with stronger condition that succesive
-    /// elements must be greater than those before them
-    template <typename C>
-    bool is_strictly_monotonic_increasing(C const& c)
-    {
-        using value_type = typename C::value_type;
-        return std::is_sorted(
-            c.begin(),
-            c.end(),
-            [] (value_type const& lhs, value_type const& rhs) {
-                return lhs <= rhs;
-            }
-        );
+    auto i = value_type(1);
+    auto it = std::find_if(
+        c.begin()+1, c.end(), [&i](value_type v) { return v>=(i++); }
+    );
+    return it==c.end();
+}
+
+template <typename C>
+bool is_positive(C const& c)
+{
+    static_assert(
+        std::is_integral<typename C::value_type>::value,
+        "is_positive only applies to integral types"
+    );
+    for(auto v : c) {
+        if(v<1) {
+            return false;
+        }
     }
-
-    template <typename C>
-    bool is_strictly_monotonic_decreasing(C const& c)
-    {
-        using value_type = typename C::value_type;
-        return std::is_sorted(
-            c.begin(),
-            c.end(),
-            [] (value_type const& lhs, value_type const& rhs) {
-                return lhs >= rhs;
-            }
-        );
+    return true;
+}
+
+template<typename C>
+bool has_contiguous_segments(const C &parent_index)
+{
+    static_assert(
+        std::is_integral<typename C::value_type>::value,
+        "integral type required"
+    );
+
+    if (!is_minimal_degree(parent_index)) {
+        return false;
     }
 
-    template <
-        typename C,
-        typename = typename std::enable_if<std::is_integral<typename C::value_type>::value>
-    >
-    bool is_minimal_degree(C const& c)
-    {
-        static_assert(
-            std::is_integral<typename C::value_type>::value,
-            "is_minimal_degree only applies to integral types"
-        );
-
-        if(c.size()==0u) {
-            return true;
-        }
+    int n = parent_index.size();
+    std::vector<bool> is_leaf(n, false);
 
-        using value_type = typename C::value_type;
-        if(c[0] != value_type(0)) {
+    for(auto i=1; i<n; ++i) {
+        auto p = parent_index[i];
+        if(is_leaf[p]) {
             return false;
         }
-        auto i = value_type(1);
-        auto it = std::find_if(
-            c.begin()+1, c.end(), [&i](value_type v) { return v>=(i++); }
-        );
-        return it==c.end();
-    }
 
-    template <typename C>
-    bool is_positive(C const& c)
-    {
-        static_assert(
-            std::is_integral<typename C::value_type>::value,
-            "is_positive only applies to integral types"
-        );
-        for(auto v : c) {
-            if(v<1) {
-                return false;
-            }
+        if(p != i-1) {
+            // we have a branch and i-1 is a leaf node
+            is_leaf[i-1] = true;
         }
-        return true;
     }
 
-    template<typename C>
-    bool has_contiguous_segments(const C &parent_index)
-    {
-        static_assert(
-            std::is_integral<typename C::value_type>::value,
-            "integral type required"
-        );
+    return true;
+}
 
-        if (!is_minimal_degree(parent_index)) {
-            return false;
-        }
+template<typename C>
+std::vector<typename C::value_type> child_count(const C &parent_index)
+{
+    static_assert(
+        std::is_integral<typename C::value_type>::value,
+        "integral type required"
+    );
 
-        int n = parent_index.size();
-        std::vector<bool> is_leaf(n, false);
+    std::vector<typename C::value_type> count(parent_index.size(), 0);
+    for (std::size_t i = 1; i < parent_index.size(); ++i) {
+        ++count[parent_index[i]];
+    }
 
-        for(auto i=1; i<n; ++i) {
-            auto p = parent_index[i];
-            if(is_leaf[p]) {
-                return false;
-            }
+    return count;
+}
 
-            if(p != i-1) {
-                // we have a branch and i-1 is a leaf node
-                is_leaf[i-1] = true;
-            }
-        }
+template<typename C>
+std::vector<typename C::value_type> branches(const C& parent_index)
+{
+    static_assert(
+        std::is_integral<typename C::value_type>::value,
+        "integral type required"
+    );
 
-        return true;
+    EXPECTS(has_contiguous_segments(parent_index));
+
+    std::vector<typename C::value_type> branch_index;
+    if (parent_index.empty()) {
+        return branch_index;
     }
 
-    template<typename C>
-    std::vector<typename C::value_type> child_count(const C &parent_index)
-    {
-        static_assert(
-            std::is_integral<typename C::value_type>::value,
-            "integral type required"
-        );
-
-        std::vector<typename C::value_type> count(parent_index.size(), 0);
-        for (std::size_t i = 1; i < parent_index.size(); ++i) {
-            ++count[parent_index[i]];
+    auto num_child = child_count(parent_index);
+    branch_index.push_back(0);
+    for (std::size_t i = 1; i < parent_index.size(); ++i) {
+        auto p = parent_index[i];
+        if (num_child[p] > 1 || parent_index[p] == p) {
+            // parent_index[p] == p -> parent_index[i] is the soma
+            branch_index.push_back(i);
         }
-
-        return count;
     }
 
-    template<typename C>
-    std::vector<typename C::value_type> branches(const C &parent_index)
-    {
-        static_assert(
-            std::is_integral<typename C::value_type>::value,
-            "integral type required"
-        );
-
-        EXPECTS(has_contiguous_segments(parent_index));
-
-        auto num_child = child_count(parent_index);
-        std::vector<typename C::value_type> branch_runs(
-            parent_index.size(), 0
-        );
-
-        std::size_t num_branches = (num_child[0] == 1) ? 1 : 0;
-        for (std::size_t i = 1; i < parent_index.size(); ++i) {
-            auto p = parent_index[i];
-            if (num_child[p] > 1) {
-                ++num_branches;
-            }
-
-            branch_runs[i] = num_branches;
-        }
+    branch_index.push_back(parent_index.size());
+    return branch_index;
+}
 
-        return branch_runs;
-    }
 
-    template<typename C>
-    bool is_sorted(const C& c)
-    {
-        return std::is_sorted(c.begin(), c.end());
+template<typename C>
+std::vector<typename C::value_type> expand_branches(const C& branch_index)
+{
+    static_assert(
+        std::is_integral<typename C::value_type>::value,
+        "integral type required"
+    );
+
+    if (branch_index.empty())
+        return {};
+
+    std::vector<typename C::value_type> expanded(branch_index.back());
+    for (std::size_t i = 0; i < branch_index.size()-1; ++i) {
+        for (std::size_t j = branch_index[i]; j < branch_index[i+1]; ++j) {
+            expanded[j] = i;
+        }
     }
 
-    template<typename C>
-    bool is_unique(const C& c)
-    {
-        return std::adjacent_find(c.begin(), c.end()) == c.end();
+    return expanded;
+}
+
+template<typename C>
+typename C::value_type find_branch(const C& branch_index,
+                                   typename C::value_type nid)
+{
+    using value_type = typename C::value_type;
+    static_assert(
+        std::is_integral<value_type>::value,
+        "integral type required"
+    );
+
+    auto it =  std::find_if(
+        branch_index.begin(), branch_index.end(),
+        [nid](const value_type &v) { return v > nid; }
+    );
+
+    return it - branch_index.begin() - 1;
+}
+
+
+template<typename C>
+std::vector<typename C::value_type> make_parent_index(
+    const C& parent_index, const C& branch_index)
+{
+    static_assert(
+        std::is_integral<typename C::value_type>::value,
+        "integral type required"
+    );
+
+    if (parent_index.empty() && branch_index.empty())
+        return {};
+
+    EXPECTS(parent_index.size() == branch_index.back());
+    EXPECTS(has_contiguous_segments(parent_index));
+    EXPECTS(is_strictly_monotonic_increasing(branch_index));
+
+    // expand the branch index
+    auto expanded_branch = expand_branches(branch_index);
+
+    std::vector<typename C::value_type> new_parent_index;
+    for (std::size_t i = 0; i < branch_index.size()-1; ++i) {
+        auto p = parent_index[branch_index[i]];
+        new_parent_index.push_back(expanded_branch[p]);
     }
 
-    /// Return and index that maps entries in sub to their corresponding
-    /// values in super, where sub is a subset of super.
-    ///
-    /// Both sets are sorted and have unique entries.
-    /// Complexity is O(n), where n is size of super
-    template<typename C>
-    // C::iterator models forward_iterator
-    // C::value_type is_integral
-    C index_into(const C& super, const C& sub)
-    {
-        //EXPECTS {s \in super : \forall s \in sub};
-        EXPECTS(is_unique(super) && is_unique(sub));
-        EXPECTS(is_sorted(super) && is_sorted(sub));
-        EXPECTS(sub.size() <= super.size());
-
-        static_assert(
-            std::is_integral<typename C::value_type>::value,
-            "index_into only applies to integral types"
-        );
-
-        C out(sub.size()); // out will have one entry for each index in sub
-
-        auto sub_it=sub.begin();
-        auto super_it=super.begin();
-        auto sub_idx=0u, super_idx = 0u;
-
-        while(sub_it!=sub.end() && super_it!=super.end()) {
-            if(*sub_it==*super_it) {
-                out[sub_idx] = super_idx;
-                ++sub_it; ++sub_idx;
-            }
-            ++super_it; ++super_idx;
+    return new_parent_index;
+}
+
+
+template<typename C>
+bool is_sorted(const C& c)
+{
+    return std::is_sorted(c.begin(), c.end());
+}
+
+template<typename C>
+bool is_unique(const C& c)
+{
+    return std::adjacent_find(c.begin(), c.end()) == c.end();
+}
+
+/// Return and index that maps entries in sub to their corresponding
+/// values in super, where sub is a subset of super.
+///
+/// Both sets are sorted and have unique entries.
+/// Complexity is O(n), where n is size of super
+template<typename C>
+// C::iterator models forward_iterator
+// C::value_type is_integral
+C index_into(const C& super, const C& sub)
+{
+    //EXPECTS {s \in super : \forall s \in sub};
+    EXPECTS(is_unique(super) && is_unique(sub));
+    EXPECTS(is_sorted(super) && is_sorted(sub));
+    EXPECTS(sub.size() <= super.size());
+
+    static_assert(
+        std::is_integral<typename C::value_type>::value,
+        "index_into only applies to integral types"
+    );
+
+    C out(sub.size()); // out will have one entry for each index in sub
+
+    auto sub_it=sub.begin();
+    auto super_it=super.begin();
+    auto sub_idx=0u, super_idx = 0u;
+
+    while(sub_it!=sub.end() && super_it!=super.end()) {
+        if(*sub_it==*super_it) {
+            out[sub_idx] = super_idx;
+            ++sub_it; ++sub_idx;
         }
+        ++super_it; ++super_idx;
+    }
 
-        EXPECTS(sub_idx==sub.size());
+    EXPECTS(sub_idx==sub.size());
 
-        return out;
-    }
+    return out;
+}
 
 } // namespace algorithms
 } // namespace mc
diff --git a/tests/test_algorithms.cpp b/tests/test_algorithms.cpp
index 584bdf8ad10db170b44604d3db8f8047d3c29860..ec25bd42c0941b8324fbea3288ed5b4f1d1f7a59 100644
--- a/tests/test_algorithms.cpp
+++ b/tests/test_algorithms.cpp
@@ -369,11 +369,11 @@ TEST(algorithms, branches)
 
     {
         //
-        //        0
-        //       /|\.
-        //      1 4 6
-        //     /  |  \.
-        //    2   5   7
+        //        0                        0
+        //       /|\.                     /|\.
+        //      1 4 6                    1 2 3
+        //     /  |  \.           =>        / \.
+        //    2   5   7                    4   5
         //   /         \.
         //  3           8
         //             / \.
@@ -386,58 +386,100 @@ TEST(algorithms, branches)
         std::vector<int> parent_index =
             { 0, 0, 1, 2, 0, 4, 0, 6, 7, 8, 9, 8, 11, 12 };
         std::vector<int> expected_branches =
-            { 0, 1, 1, 1, 2, 2, 3, 3, 3, 4, 4, 5, 5, 5 };
+            { 0, 1, 4, 6, 9, 11, 14 };
+        std::vector<int> expected_parent_index =
+            { 0, 0, 0, 0, 3, 3 };
 
         auto actual_branches = algorithms::branches(parent_index);
         EXPECT_EQ(expected_branches, actual_branches);
+
+        auto actual_parent_index =
+            algorithms::make_parent_index(parent_index, actual_branches);
+        EXPECT_EQ(expected_parent_index, actual_parent_index);
+
+        // Check find_branch
+        EXPECT_EQ(0, algorithms::find_branch(actual_branches,  0));
+        EXPECT_EQ(1, algorithms::find_branch(actual_branches,  1));
+        EXPECT_EQ(1, algorithms::find_branch(actual_branches,  2));
+        EXPECT_EQ(1, algorithms::find_branch(actual_branches,  3));
+        EXPECT_EQ(2, algorithms::find_branch(actual_branches,  4));
+        EXPECT_EQ(2, algorithms::find_branch(actual_branches,  5));
+        EXPECT_EQ(3, algorithms::find_branch(actual_branches,  6));
+        EXPECT_EQ(3, algorithms::find_branch(actual_branches,  7));
+        EXPECT_EQ(3, algorithms::find_branch(actual_branches,  8));
+        EXPECT_EQ(4, algorithms::find_branch(actual_branches,  9));
+        EXPECT_EQ(4, algorithms::find_branch(actual_branches, 10));
+        EXPECT_EQ(5, algorithms::find_branch(actual_branches, 11));
+        EXPECT_EQ(5, algorithms::find_branch(actual_branches, 12));
+        EXPECT_EQ(5, algorithms::find_branch(actual_branches, 13));
+        EXPECT_EQ(6, algorithms::find_branch(actual_branches, 55));
+
+        // Check expand_branches
+        auto expanded = algorithms::expand_branches(actual_branches);
+        EXPECT_EQ(parent_index.size(), expanded.size());
+        for (std::size_t i = 0; i < parent_index.size(); ++i) {
+            EXPECT_EQ(algorithms::find_branch(actual_branches, i),
+                      expanded[i]);
+        }
     }
 
     {
         //
-        //    0
-        //    |
-        //    1
+        //    0      0
+        //    |      |
+        //    1  =>  1
         //    |
         //    2
         //    |
         //    3
         //
-        std::vector<int> parent_index =
-            { 0, 0, 1, 2 };
-        std::vector<int> expected_branches =
-            { 0, 1, 1, 1 };
+        std::vector<int> parent_index          = { 0, 0, 1, 2 };
+        std::vector<int> expected_branches     = { 0, 1, 4 };
+        std::vector<int> expected_parent_index = { 0, 0 };
 
         auto actual_branches = algorithms::branches(parent_index);
         EXPECT_EQ(expected_branches, actual_branches);
+
+        auto actual_parent_index =
+            algorithms::make_parent_index(parent_index, actual_branches);
+        EXPECT_EQ(expected_parent_index, actual_parent_index);
     }
 
     {
         //
-        //    0
-        //    |
-        //    1
-        //    |
-        //    2
+        //    0           0
+        //    |           |
+        //    1     =>    1
+        //    |          / \.
+        //    2         2   3
         //   / \.
         //  3   4
         //       \.
         //        5
         //
-        std::vector<int> parent_index =
-            { 0, 0, 1, 2, 2, 4 };
-        std::vector<int> expected_branches =
-            { 0, 1, 1, 2, 3, 3 };
+        std::vector<int> parent_index          = { 0, 0, 1, 2, 2, 4 };
+        std::vector<int> expected_branches     = { 0, 1, 3, 4, 6 };
+        std::vector<int> expected_parent_index = { 0, 0, 1, 1 };
 
         auto actual_branches = algorithms::branches(parent_index);
         EXPECT_EQ(expected_branches, actual_branches);
+
+        auto actual_parent_index =
+            algorithms::make_parent_index(parent_index, actual_branches);
+        EXPECT_EQ(expected_parent_index, actual_parent_index);
     }
 
     {
-        std::vector<int> parent_index = { 0 };
-        std::vector<int> expected_branches = { 0 };
+        std::vector<int> parent_index          = { 0 };
+        std::vector<int> expected_branches     = { 0, 1 };
+        std::vector<int> expected_parent_index = { 0 };
 
         auto actual_branches = algorithms::branches(parent_index);
         EXPECT_EQ(expected_branches, actual_branches);
+
+        auto actual_parent_index =
+            algorithms::make_parent_index(parent_index, actual_branches);
+        EXPECT_EQ(expected_parent_index, actual_parent_index);
     }
 }