diff --git a/QuadTree.Tests/QuadTree.Tests.fsproj b/QuadTree.Tests/QuadTree.Tests.fsproj
index 9d6d3aa..8697b9a 100644
--- a/QuadTree.Tests/QuadTree.Tests.fsproj
+++ b/QuadTree.Tests/QuadTree.Tests.fsproj
@@ -10,6 +10,7 @@
     <Compile Include="Tests.Vector.fs" />
     <Compile Include="Tests.Matrix.fs" />
     <Compile Include="Tests.LinearAlgebra.fs" />
+    <Compile Include="Tests.BFS.fs" />
   </ItemGroup>
 
   <ItemGroup>
diff --git a/QuadTree.Tests/Tests.BFS.fs b/QuadTree.Tests/Tests.BFS.fs
new file mode 100644
index 0000000..cf652d0
--- /dev/null
+++ b/QuadTree.Tests/Tests.BFS.fs
@@ -0,0 +1,71 @@
+module Graph.BFS.Tests
+
+open System
+open Xunit
+
+open Matrix
+open Vector
+open Common
+
+(*
+1,N,N,N
+,
+N,1,1,N
+3,N,2,3
+N,N,N,2
+N,N,3,N
+=>
+0,1,1,2
+*)
+[<Fact>]
+let ``Simple level bfs.`` () =
+    let graph =
+        let tree =
+            Matrix.qtree.Node(
+                Matrix.qtree.Node(
+                    Matrix.qtree.Leaf(UserValue(None)),
+                    Matrix.qtree.Leaf(UserValue(Some(1))),
+                    Matrix.qtree.Leaf(UserValue(Some(3))),
+                    Matrix.qtree.Leaf(UserValue(None))
+                ),
+                Matrix.qtree.Node(
+                    Matrix.qtree.Leaf(UserValue(Some(1))),
+                    Matrix.qtree.Leaf(UserValue(None)),
+                    Matrix.qtree.Leaf(UserValue(Some(2))),
+                    Matrix.qtree.Leaf(UserValue(Some(3)))
+                ),
+                Matrix.qtree.Leaf(UserValue(None)),
+                Matrix.qtree.Node(
+                    Matrix.qtree.Leaf(UserValue(None)),
+                    Matrix.qtree.Leaf(UserValue(Some(2))),
+                    Matrix.qtree.Leaf(UserValue(Some(3))),
+                    Matrix.qtree.Leaf(UserValue(None))
+                )
+            )
+
+        let store = Matrix.Storage(4UL<storageVSize>, 4UL<storageHSize>, tree)
+        SparseMatrix(4UL<nrows>, 4UL<ncols>, 9UL<nvals>, store)
+
+    let startVertices =
+        let tree =
+            Vector.btree.Node(
+                Vector.btree.Node(Vector.btree.Leaf(UserValue(Some(1UL))), Vector.btree.Leaf(UserValue(None))),
+                Vector.btree.Leaf(UserValue(None))
+            )
+
+        let store = Vector.Storage(4UL<storageSize>, tree)
+        SparseVector(4UL<dataLength>, 1UL<nvals>, store)
+
+    let expected =
+        let tree =
+            Vector.btree.Node(
+                Vector.btree.Node(Vector.btree.Leaf(UserValue(Some(0UL))), Vector.btree.Leaf(UserValue(Some(1UL)))),
+                Vector.btree.Node(Vector.btree.Leaf(UserValue(Some(1UL))), Vector.btree.Leaf(UserValue(Some(2UL))))
+            )
+
+        let store = Vector.Storage(4UL<storageSize>, tree)
+        Result.Success(SparseVector(4UL<dataLength>, 4UL<nvals>, store))
+
+    let actual = Graph.BFS.bfs_level graph startVertices
+
+    Assert.Equal(expected, actual)
diff --git a/QuadTree.Tests/Tests.LinearAlgebra.fs b/QuadTree.Tests/Tests.LinearAlgebra.fs
index 7b6196c..f29b2a2 100644
--- a/QuadTree.Tests/Tests.LinearAlgebra.fs
+++ b/QuadTree.Tests/Tests.LinearAlgebra.fs
@@ -76,9 +76,7 @@ let ``Simple vxm. All sizes are power of two.`` () =
 
     let actual = LinearAlgebra.vxm op_add op_mult v m
 
-    let eq = actual = expected
-
-    Assert.True(eq)
+    Assert.Equal(expected, actual)
 
 (*
 2,2,2,D
@@ -158,9 +156,7 @@ let ``Simple vxm. 3 * (3x4)`` () =
 
     let actual = LinearAlgebra.vxm op_add op_mult v m
 
-    let eq = actual = expected
-
-    Assert.True(eq)
+    Assert.Equal(expected, actual)
 
 
 (*
@@ -232,6 +228,4 @@ let ``Simple vxm. 4 * (4x3).`` () =
 
     let actual = LinearAlgebra.vxm op_add op_mult v m
 
-    let eq = actual = expected
-
-    Assert.True(eq)
+    Assert.Equal(expected, actual)
diff --git a/QuadTree.Tests/Tests.Matrix.fs b/QuadTree.Tests/Tests.Matrix.fs
index f5fcc84..80f642b 100644
--- a/QuadTree.Tests/Tests.Matrix.fs
+++ b/QuadTree.Tests/Tests.Matrix.fs
@@ -103,9 +103,7 @@ let ``Simple Matrix.map2. Square where number of cols and rows are power of two.
 
     let actual = Matrix.map2 m1 m2 f
 
-    let eq = actual = expected
-
-    Assert.True(eq)
+    Assert.Equal(expected, actual)
 
 (*
 N,1,1,D
@@ -223,9 +221,7 @@ let ``Simple Matrix.map2. Square where number of cols and rows are not power of
 
     let actual = Matrix.map2 m1 m2 f
 
-    let eq = actual = expected
-
-    Assert.True(eq)
+    Assert.Equal(expected, actual)
 
 [<Fact>]
 let ``Conversion identity`` () =
diff --git a/QuadTree.Tests/Tests.Vector.fs b/QuadTree.Tests/Tests.Vector.fs
index 6aa6d57..ad1a525 100644
--- a/QuadTree.Tests/Tests.Vector.fs
+++ b/QuadTree.Tests/Tests.Vector.fs
@@ -14,6 +14,70 @@ let printVector (vector: SparseVector<_>) =
     printfn "      Size: %A" vector.storage.size
     printfn "      Data: %A" vector.storage.data
 
+
+[<Fact>]
+let ``Simple Vector.map. Length is power of two.`` () =
+    let v =
+        let tree =
+            Vector.btree.Node(
+                Vector.btree.Node(Vector.btree.Leaf(UserValue(Some(1))), Vector.btree.Leaf(UserValue(None))),
+                Vector.btree.Leaf(UserValue(Some(2)))
+            )
+
+        let store = Storage(8UL<storageSize>, tree)
+        SparseVector(8UL<dataLength>, 6UL<nvals>, store)
+
+    let f x =
+        match x with
+        | Some(a) -> Some(a * 2)
+        | _ -> None
+
+    let expected =
+        let tree =
+            Vector.btree.Node(
+                Vector.btree.Node(Vector.btree.Leaf(UserValue(Some(2))), Vector.btree.Leaf(UserValue(None))),
+                Vector.btree.Leaf(UserValue(Some(4)))
+            )
+
+        let store = Storage(8UL<storageSize>, tree)
+        SparseVector(8UL<dataLength>, 6UL<nvals>, store)
+
+    let actual = Vector.map v f
+
+    Assert.Equal(expected, actual)
+
+[<Fact>]
+let ``Simple Vector.map. Length is not power of two.`` () =
+    let v =
+        let tree =
+            Vector.btree.Node(
+                Vector.btree.Node(Vector.btree.Leaf(UserValue(Some(1))), Vector.btree.Leaf(UserValue(None))),
+                Vector.btree.Node(Vector.btree.Leaf(UserValue(None)), Vector.btree.Leaf(Dummy))
+            )
+
+        let store = Storage(8UL<storageSize>, tree)
+        SparseVector(6UL<dataLength>, 2UL<nvals>, store)
+
+    let f x =
+        match x with
+        | Some(a) -> Some(a * 2)
+        | _ -> None
+
+    let expected =
+        let tree =
+            Vector.btree.Node(
+                Vector.btree.Node(Vector.btree.Leaf(UserValue(Some(2))), Vector.btree.Leaf(UserValue(None))),
+                Vector.btree.Node(Vector.btree.Leaf(UserValue(None)), Vector.btree.Leaf(Dummy))
+            )
+
+        let store = Storage(8UL<storageSize>, tree)
+        SparseVector(6UL<dataLength>, 2UL<nvals>, store)
+
+    let actual = Vector.map v f
+
+    Assert.Equal(expected, actual)
+
+
 [<Fact>]
 let ``Simple Vector.map2. Length is power of two.`` () =
     let v1 =
@@ -53,9 +117,7 @@ let ``Simple Vector.map2. Length is power of two.`` () =
 
     let actual = Vector.map2 v1 v2 f
 
-    let eq = actual = expected
-
-    Assert.True(eq)
+    Assert.Equal(expected, actual)
 
 [<Fact>]
 let ``Simple Vector.map2. Length is not power of two.`` () =
@@ -96,9 +158,7 @@ let ``Simple Vector.map2. Length is not power of two.`` () =
 
     let actual = Vector.map2 v1 v2 f
 
-    let eq = actual = expected
-
-    Assert.True(eq)
+    Assert.Equal(expected, actual)
 
 [<Fact>]
 let ``Conversion identity`` () =
diff --git a/QuadTree/BFS.fs b/QuadTree/BFS.fs
index f1d5ce6..b2ffc3c 100644
--- a/QuadTree/BFS.fs
+++ b/QuadTree/BFS.fs
@@ -1,13 +1,48 @@
 module Graph.BFS
 
-// let bfs_general op_add graph startVertices =
-//     let rec inner frontier visited =
-//         if Vector.nvals frontier > 0 then
-//             let new_frontier = LinearAlgebra.vxm frontier graph
-//             let frontier = Vector.mask new_frontier visited (fun x -> x.IsNone)
-//             let visited = Vector.map2 visited new_frontier op_add
-//             inner frontier visited
-//         else
-//             visited
-
-//     inner startVertices startVertices
+open Common
+
+type Error<'t1, 't2> =
+    | NewFrontierCalculationProblem of LinearAlgebra.Error<'t1, 't2, 't1>
+    | FrontierCalculationProblem of Vector.Error<'t1, 't1>
+    | VisitedCalculationProblem of Vector.Error<'t1, 't1>
+
+let bfs_level graph startVertices =
+    let rec inner level (frontier: Vector.SparseVector<_>) (visited: Vector.SparseVector<_>) =
+        if frontier.nvals > 0UL<nvals> then
+            let op_add x y =
+                match (x, y) with
+                | Some(v), _
+                | _, Some(v) -> Some(v)
+                | _ -> None
+
+            let op_mult x y =
+                match (x, y) with
+                | Some(v), Some(_) -> Some(v)
+                | _ -> None
+
+            let new_frontier = LinearAlgebra.vxm op_add op_mult frontier graph
+
+            match new_frontier with
+            | Result.Failure(e) -> Result.Failure(NewFrontierCalculationProblem(e))
+            | Result.Success(new_frontier) ->
+                let frontier = Vector.mask new_frontier visited (fun x -> x.IsNone)
+
+                match frontier with
+                | Result.Failure(e) -> Result.Failure(FrontierCalculationProblem(e))
+                | Result.Success(frontier) ->
+                    let op_add x y =
+                        match (x, y) with
+                        | (Some(_), _) -> x
+                        | (None, Some(_)) -> Some(level)
+                        | _ -> None
+
+                    let visited = Vector.map2 visited new_frontier op_add
+
+                    match visited with
+                    | Result.Failure(e) -> Result.Failure(VisitedCalculationProblem(e))
+                    | Result.Success(visited) -> inner (level + 1UL) frontier visited
+        else
+            Result.Success visited
+
+    inner 1UL startVertices (Vector.map startVertices (Option.map (fun x -> 0UL)))
diff --git a/QuadTree/Vector.fs b/QuadTree/Vector.fs
index dd0d8a1..99396da 100644
--- a/QuadTree/Vector.fs
+++ b/QuadTree/Vector.fs
@@ -29,8 +29,9 @@ type SparseVector<'value> =
           nvals = _nvals
           storage = _storage }
 
-type Error<'value1, 'value2> = InconsistentSizeOfArguments of SparseVector<'value1> * SparseVector<'value2>
-
+type Error<'value1, 'value2> =
+    | InconsistentStructureOfStorages of btree<Option<'value1>> * btree<Option<'value2>>
+    | InconsistentSizeOfArguments of SparseVector<'value1> * SparseVector<'value2>
 
 (*
 let foldValues state f tree =
@@ -100,24 +101,46 @@ let toCoordinateList (vector: SparseVector<'a>) =
 
     CoordinateList(length, lst)
 
+let map (vector: SparseVector<'a>) f =
+    let rec inner (size: uint64<storageSize>) vector =
+        match vector with
+        | Node(x1, x2) ->
+            let t1, nvals1 = inner (size / 2UL) x1
+            let t2, nvals2 = inner (size / 2UL) x2
+            (mkNode t1 t2), nvals1 + nvals2
+        | Leaf(Dummy) -> Leaf(Dummy), 0UL<nvals>
+        | Leaf(UserValue(v)) ->
+            let res = f v
+
+            let nnz =
+                match res with
+                | None -> 0UL<nvals>
+                | _ -> (uint64 size) * 1UL<nvals>
+
+            Leaf(UserValue(res)), nnz
+
+    let storage, nvals = inner vector.storage.size vector.storage.data
+
+    SparseVector(vector.length, nvals, (Storage(vector.storage.size, storage)))
+
 let map2 (vector1: SparseVector<'a>) (vector2: SparseVector<'b>) f =
     let len1 = vector1.length
 
     let rec inner (size: uint64<storageSize>) vector1 vector2 =
+        let _do x1 x2 y1 y2 =
+            let new_size = size / 2UL
+
+            match (inner new_size x1 y1), (inner new_size x2 y2) with
+            | Result.Success((t1, nvals1)), Result.Success((t2, nvals2)) ->
+                ((mkNode t1 t2), nvals1 + nvals2) |> Result.Success
+            | Result.Failure(e), _
+            | _, Result.Failure(e) -> Result.Failure(e)
+
         match (vector1, vector2) with
-        | Node(t1, t2), Leaf(_) ->
-            let new_t1, nvals1 = inner (size / 2UL) t1 vector2
-            let new_t2, nvals2 = inner (size / 2UL) t2 vector2
-            (mkNode new_t1 new_t2), nvals1 + nvals2
-        | Leaf(_), Node(t1, t2) ->
-            let new_t1, nvals1 = inner (size / 2UL) vector1 t1
-            let new_t2, nvals2 = inner (size / 2UL) vector1 t2
-            (mkNode new_t1 new_t2), nvals1 + nvals2
-        | Node(t1, t2), Node(t3, t4) ->
-            let new_t1, nvals1 = inner (size / 2UL) t1 t3
-            let new_t2, nvals2 = inner (size / 2UL) t2 t4
-            (mkNode new_t1 new_t2), nvals1 + nvals2
-        | Leaf(Dummy), Leaf(Dummy) -> Leaf(Dummy), 0UL<nvals>
+        | Node(x1, x2), Leaf(_) -> _do x1 x2 vector2 vector2
+        | Leaf(_), Node(y1, y2) -> _do vector1 vector1 y1 y2
+        | Node(x1, x2), Node(y1, y2) -> _do x1 x2 y1 y2
+        | Leaf(Dummy), Leaf(Dummy) -> Result.Success(Leaf(Dummy), 0UL<nvals>)
         | Leaf(UserValue(v1)), Leaf(UserValue(v2)) ->
             let res = f v1 v2
 
@@ -126,14 +149,15 @@ let map2 (vector1: SparseVector<'a>) (vector2: SparseVector<'b>) f =
                 | None -> 0UL<nvals>
                 | _ -> (uint64 size) * 1UL<nvals>
 
-            Leaf(UserValue(res)), nnz
-
-    if len1 = vector2.length then
+            Result.Success(Leaf(UserValue(res)), nnz)
 
-        let storage, nvals =
-            inner vector1.storage.size vector1.storage.data vector2.storage.data
+        | (x, y) -> Result.Failure <| Error.InconsistentStructureOfStorages(x, y)
 
-        Result.Success(SparseVector(len1, nvals, (Storage(vector1.storage.size, storage))))
+    if len1 = vector2.length then
+        match inner vector1.storage.size vector1.storage.data vector2.storage.data with
+        | Result.Failure(e) -> Result.Failure(e)
+        | Result.Success((storage, nvals)) ->
+            Result.Success(SparseVector(len1, nvals, (Storage(vector1.storage.size, storage))))
     else
         Result.Failure <| Error.InconsistentSizeOfArguments(vector1, vector2)