diff --git a/src/afgl/test_1.py b/src/afgl/ex_1.py
similarity index 85%
rename from src/afgl/test_1.py
rename to src/afgl/ex_1.py
index 8ba988d..0c1490a 100644
--- a/src/afgl/test_1.py
+++ b/src/afgl/ex_1.py
@@ -14,6 +14,7 @@ from afgl.util.plot import latex_log_formatter
 
 
 def plot_graphs(G_ER, G_Sensor, s: np.ndarray, N: int, p: float) -> None:
+    """Visualization of signal being filtered of two different types of graphs."""
     fig, axs = plt.subplots(2, 2, figsize=(6.6, 5))
 
     # Set coordinates
@@ -52,7 +53,7 @@ def plot_graphs(G_ER, G_Sensor, s: np.ndarray, N: int, p: float) -> None:
 
 
 def g_extended(t: np.ndarray) -> np.ndarray:
-    return np.sin(0.5 * np.pi * np.cos(np.pi * t) ** 2)
+    return np.sin(1 / 2 * np.pi * (np.cos(np.pi * t) ** 2))
 
 
 """
@@ -70,19 +71,21 @@ def g(T: np.ndarray) -> np.ndarray:
     return np.where(Chi, g_extended(T), 0)
 
 
-"""
-Computes the approximation g_M (see [1]) using Lanczos
-"""
-
-
 def compute_g_M(
     V: np.ndarray, alp: np.ndarray, beta: np.ndarray, s: np.ndarray
 ) -> np.ndarray:
+    """
+    Computes the approximation g_M (see [1]) using Lanczos
+    """
     M = len(alp)
     e_1 = np.zeros(M)
     e_1[0] = 1
     T = build_T_matrix(alp, beta)
-    y = LA.norm(s) * (g(T) @ e_1)
+
+    eigvals, eigvecs = LA.eigh(T)
+    g_T = eigvecs @ np.diag(g(eigvals)) @ eigvecs.T
+
+    y = LA.norm(s) * (g_T @ e_1)
     return V @ y
 
 
@@ -139,14 +142,14 @@ def run_comparison_1_for_graph(
     j = 3
     V, alp, beta = lanczos(L, s, M_MAX + j)
 
-    lanczos_err = np.zeros(M_MAX + j)
-    true_err = np.zeros(M_MAX + j)
+    lanczos_err = np.zeros(M_MAX)
+    true_err = np.zeros(M_MAX)
 
     GLs = filter_signal_with_fourier(G, s)
 
-    for M in range(2, M_MAX + j):
-        g_M = compute_g_M(V[:, 0:M], alp[0:M], beta[0 : M - 1], s)
-        g_Mj = compute_g_M(V[:, 0 : M + j], alp[0 : M + j], beta[0 : M + j - 1], s)
+    for M in range(1, M_MAX + 1):
+        g_M = compute_g_M(V[:, :M], alp[:M], beta[: M - 1], s)
+        g_Mj = compute_g_M(V[:, : M + j], alp[: M + j], beta[: M + j - 1], s)
 
         lanczos_err[M - 1] = LA.norm(g_Mj - g_M)
         true_err[M - 1] = LA.norm(GLs - g_M)
@@ -157,10 +160,10 @@ def run_comparison_1_for_graph(
 def run() -> None:
     """Ripete il test corrispondente ad Example 1 dell'articolo limitandosi al
     metodo di Lanczos (no Chebyshev) e utilizzando come funzione g(t) = sin(0.5π
-    cos(πt)2) * \chi_{[-0.5, 0.5]}.
+    cos(πt)2) * chi_{[-0.5, 0.5]}.
     """
     N = 500
-    M_MAX = 200
+    M = 200
     p = 0.04
 
     s = np.random.randint(1, 10000, N).astype(float)
@@ -170,8 +173,8 @@ def run() -> None:
     G_ER = graphs.ErdosRenyi(N, p)
     G_S = graphs.Sensor(N)
 
-    l_err_ER, t_err_ER = run_comparison_1_for_graph(G_ER, s, M_MAX)
-    l_err_S, t_err_S = run_comparison_1_for_graph(G_S, s, M_MAX)
+    l_err_ER, t_err_ER = run_comparison_1_for_graph(G_ER, s, M)
+    l_err_S, t_err_S = run_comparison_1_for_graph(G_S, s, M)
 
     plot_error_comparison(l_err_ER, t_err_ER, l_err_S, t_err_S)
-    plot_graphs(G_ER, G_S, s, N, p)
+    # plot_graphs(G_ER, G_S, s, N, p)
diff --git a/src/afgl/test_2.py b/src/afgl/ex_2.py
similarity index 100%
rename from src/afgl/test_2.py
rename to src/afgl/ex_2.py
diff --git a/src/afgl/main.py b/src/afgl/main.py
index b4076e0..7d3f23f 100644
--- a/src/afgl/main.py
+++ b/src/afgl/main.py
@@ -1,13 +1,13 @@
 import sys
 
-import afgl.test_2 as t_2
+import afgl.ex_1 as ex_1
 from afgl.util.plot import plot_setup
 
 
 def run() -> None:
     plot_setup()
-    # t_1.run()
-    t_2.run()
+    ex_1.run()
+    # t_2.run()
 
 
 if __name__ == "__main__":
diff --git a/src/afgl/util/arnoldi.py b/src/afgl/util/arnoldi.py
deleted file mode 100644
index e69de29..0000000
diff --git a/src/afgl/util/build_T_matrix.py b/src/afgl/util/build_T_matrix.py
index d274384..391ba2b 100644
--- a/src/afgl/util/build_T_matrix.py
+++ b/src/afgl/util/build_T_matrix.py
@@ -2,4 +2,14 @@ import numpy as np
 
 
 def build_T_matrix(alp, beta):
+    """Constructs Lanczos T tridiagonal matrix.
+
+    Args:
+        alp: Vector of alphas of size N
+        beta: Vector of betas of size N-1
+
+    Returns:
+        T : Matrix T
+    """
+
     return np.diag(alp) + np.diag(beta, -1) + np.diag(beta, 1)
diff --git a/src/afgl/util/lanczos.py b/src/afgl/util/lanczos.py
index 1d69ddd..a72e18f 100644
--- a/src/afgl/util/lanczos.py
+++ b/src/afgl/util/lanczos.py
@@ -2,6 +2,20 @@ import numpy as np
 import numpy.linalg as LA
 
 
+def double_orthogonalization(V, w, j):
+    # Why it works well until j+1? it should be until j-1 from Demmel
+    for _ in range(2):
+        w -= V[:, : j + 1] @ (V[:, : j + 1].T @ w)
+    return w
+
+
+def no_orthogonalization(V, w, alp, beta, j):
+    w = w - alp[j] * V[:, j]
+    if j > 0:
+        w = w - beta[j - 1] * V[:, j - 1]
+    return w
+
+
 def lanczos(L, s, M):
     """
     Classic Lanczos method (without re-orthogonalization)
@@ -30,9 +44,7 @@ def lanczos(L, s, M):
         w = L @ V[:, j]
         alp[j] = np.dot(V[:, j], w)
 
-        w = w - alp[j] * V[:, j]
-        if j > 0:
-            w = w - beta[j - 1] * V[:, j - 1]
+        w = no_orthogonalization(V, w, alp, beta, j)
 
         if j < M - 1:
             beta[j] = LA.norm(w)
diff --git a/tests/test_main.py b/tests/test_main.py
index 3c69326..351f0af 100644
--- a/tests/test_main.py
+++ b/tests/test_main.py
@@ -1,17 +1,31 @@
 import numpy as np
 import numpy.linalg as LA
+from afgl.ex_1 import compute_g_M, filter_signal_with_fourier, g
 from afgl.util.build_T_matrix import build_T_matrix
 from afgl.util.lanczos import lanczos
+from pygsp import graphs
 
-"""
-Todo: better test case
-"""
 
+def g_evaluation_should_respect_chi():
+    A = np.array([[1, 0, 1], [0, 0, 0], [1, 0, 0]])
+    expected_gA = np.array([[0, 1, 0], [1, 1, 1], [0, 1, 1]])
+    assert (expected_gA == g(A).astype(int)).all()
 
-def test_lanczos_return_correct_solution():
+
+def g_evaluation_should_return_matrix_of_zeros():
+    A = 1 / 2 * np.array([[1, 1, 1], [1, 1, 1], [1, 1, 1]])
+
+    assert (g(A).astype(int) == np.zeros((3, 3))).all()
+
+
+def test_lanczos_return_correct_solution_with_dense():
+    """Tests correctness of solution of Lx=s comparing Lanczos projected
+    solution with numpy solve function.
+    """
     N = 1000
     M = 999
 
+    # Generate a good conditioned matrix
     eigvals = np.random.uniform(10000, 100000, N)
     Q, _ = LA.qr(np.random.randn(N, N))
     L = Q @ np.diag(eigvals) @ Q.T
@@ -28,3 +42,35 @@ def test_lanczos_return_correct_solution():
     x_lanczos = V @ y
 
     assert LA.norm(x - x_lanczos) < 1e-10
+
+
+def test_function_g_with_graph_laplacian():
+    N = 1000
+    p = 0.04
+    j = 3
+
+    n = 5
+    M_VALS = 25 * (2 ** np.arange(n))
+    M_VALS = [200]
+
+    for M in M_VALS:
+        s = np.random.randint(1, 10000, N).astype(float)
+        # Normalize s as in request
+        s /= LA.norm(s)
+
+        GRAPHS = [graphs.ErdosRenyi(N, p), graphs.Sensor(N)]
+
+        for G in GRAPHS:
+            G.compute_laplacian()
+            L = G.L
+            V, alp, beta = lanczos(L, s, M + j)
+
+            GLs = filter_signal_with_fourier(G, s)
+
+            g_M = compute_g_M(V[:, 0:M], alp[0:M], beta[0 : M - 1], s)
+            g_Mj = compute_g_M(V[:, 0 : M + j], alp[0 : M + j], beta[0 : M + j - 1], s)
+
+            diff = LA.norm(g_Mj - g_M)
+            e_M = LA.norm(GLs - g_M)
+
+            assert abs(diff - e_M) < 1e-2