diff --git a/src/library/rewrite/fo_match.cpp b/src/library/rewrite/fo_match.cpp
index 47f09fcfd..6c07d77ce 100644
--- a/src/library/rewrite/fo_match.cpp
+++ b/src/library/rewrite/fo_match.cpp
@@ -10,172 +10,238 @@ Author: Soonho Kong
 #include "library/all/all.h"
 #include "library/arith/nat.h"
 #include "library/arith/arith.h"
+#include "library/rewrite/fo_match.h"
 #include "library/rewrite/rewrite.h"
+#include "library/printer.h"
+
+using std::cout;
+using std::endl;
 
 namespace lean {
 
-
-std::ostream & operator<<(std::ostream & out, expr_pair const & p) {
-    out << "("
-        << p.first << ", "
-        << p.second << ")";
+std::ostream & operator<<(std::ostream & out, subst_map & s) {
+    out << "{";
+    for (auto it = s.begin(); it != s.end(); it++) {
+        out << it->first << " => ";
+        out << it->second << "; ";
+    }
+    out << "}";
     return out;
 }
 
-bool fo_match::match_var(context & ctx, expr const & p, expr const & t, subst_map & s) {
-    std::cerr << "match_var : ("
-              << ctx << " ,"
-              << p << ", "
-              << t << ", "
-//              << s << ")"
-              << std::endl;
-    s.insert(std::make_pair(var_idx(p), t));
-    return true;
-}
-bool fo_match::match_constant(context & ctx, expr const & p, expr const & t, subst_map & s) {
-    std::cerr << "match_constant : ("
-              << ctx << " ,"
-              << p << ", "
-              << t << ", "
-//              << s << ")"
-              << std::endl;
-    if (is_constant(t) && const_name(p) == const_name(t)) {
-        return true;
+bool fo_match::match_var(expr const & p, expr const & t, unsigned o, subst_map & s) {
+    cout << "match_var : ("
+         << p << ", "
+         << t << ", "
+         << o << ", "
+         << s << ")"
+         << endl;
+
+    unsigned idx = var_idx(p);
+    if (idx < o) {
+        // Current variable is the one created by lambda inside of pattern
+        // and it is not a target of pattern matching.
+        return p == t;
     } else {
-        return false;
+        auto it = s.find(p);
+        if (it != s.end()) {
+            // This variable already has an entry in the substitution
+            // map. We need to make sure that 't' and s[idx] are the
+            // same
+            cout << "match_var exist:" << p << " |-> " << it->second << endl;
+            return it->second == t;
+        }
+        // This variable has no entry in the substituition map. Let's
+        // add one.
+        s.insert(std::make_pair(p, t));
+        cout << "match_var MATCHED : " << s << endl;
+        return true;
     }
 }
-bool fo_match::match_value(context & ctx, expr const & p, expr const & t, subst_map & s) {
-    std::cerr << "match_value : ("
-              << ctx << " ,"
-              << p << ", "
-              << t << ", "
-//              << s << ")"
-              << std::endl;
-    if (is_value(t) && to_value(p) == to_value(t)) {
-        return true;
-    } else {
-        return false;
-    }
+
+bool fo_match::match_constant(expr const & p, expr const & t, unsigned o, subst_map & s) {
+    cout << "match_constant : ("
+         << p << ", "
+         << t << ", "
+         << o << ", "
+         << s << ")"
+         << endl;
+    return p == t;
 }
-bool fo_match::match_app(context & ctx, expr const & p, expr const & t, subst_map & s) {
-    std::cerr << "match_app : ("
-              << ctx << " ,"
-              << p << ", "
-              << t << ", "
-//              << s << ")"
-              << std::endl;
+
+bool fo_match::match_value(expr const & p, expr const & t, unsigned o, subst_map & s) {
+    cout << "match_value : ("
+         << p << ", "
+         << t << ", "
+         << o << ", "
+         << s << ")"
+         << endl;
+    return p == t;
+}
+
+bool fo_match::match_app(expr const & p, expr const & t, unsigned o, subst_map & s) {
+    cout << "match_app : ("
+         << p << ", "
+         << t << ", "
+         << o << ", "
+         << s << ")"
+         << endl;
     unsigned num_p = num_args(p);
     unsigned num_t = num_args(p);
     if (num_p != num_t) {
         return false;
     }
 
-    for unsigned i = 0; i <= num_p; i++) {
-        if (!match(ctx, arg(p, i), arg(t, i), s))
+    for (unsigned i = 0; i < num_p; i++) {
+        if (!match(arg(p, i), arg(t, i), o, s))
             return false;
     }
     return true;
 }
-bool fo_match::match_lambda(context & ctx, expr const & p, expr const & t, subst_map & s) {
-    std::cerr << "match_lambda : ("
-              << ctx << " ,"
-              << p << ", "
-              << t << ", "
-//              << s << ")"
-              << std::endl;
-    // TODO(soonho)
-    std::cerr << "fun (" << abst_name(p)
-              << " : " << abst_domain(p)
-              << "), " << abst_body(p) << std::endl;
+
+bool fo_match::match_lambda(expr const & p, expr const & t, unsigned o, subst_map & s) {
+    cout << "match_lambda : ("
+         << p << ", "
+         << t << ", "
+         << o << ", "
+         << s << ")"
+         << endl;
+    cout << "fun (" << abst_name(p)
+         << " : " << abst_domain(p)
+         << "), " << abst_body(p) << endl;
     if (!is_lambda(t)) {
         return false;
-    }
+    } else {
+        // First match the domain part
+        auto p_domain = abst_domain(p);
+        auto t_domain = abst_domain(t);
+        if (!match(p_domain, t_domain, o, s))
+            return false;
 
-    return true;
+        // Then match the body part, increase offset by 1.
+        auto p_body = abst_body(p);
+        auto t_body = abst_body(t);
+        return match(p_domain, t_domain, o + 1, s);
+    }
 }
-bool fo_match::match_pi(context & ctx, expr const & p, expr const & t, subst_map & s) {
-    std::cerr << "match_pi : ("
-              << ctx << " ,"
-              << p << ", "
-              << t << ", "
-//              << s << ")"
-              << std::endl;
-    // TODO(soonho)
-    std::cerr << "Pi (" << abst_name(p)
-              << " : " << abst_domain(p)
-              << "), " << abst_body(p) << std::endl;
-    return true;
+
+bool fo_match::match_pi(expr const & p, expr const & t, unsigned o, subst_map & s) {
+    cout << "match_pi : ("
+         << p << ", "
+         << t << ", "
+         << o << ", "
+         << s << ")"
+         << endl;
+    cout << "Pi (" << abst_name(p)
+         << " : " << abst_domain(p)
+         << "), " << abst_body(p) << endl;
+
+    if (!is_pi(t)) {
+        return false;
+    } else {
+        // First match the domain part
+        auto p_domain = abst_domain(p);
+        auto t_domain = abst_domain(t);
+        if (!match(p_domain, t_domain, o, s))
+            return false;
+
+        // Then match the body part, increase offset by 1.
+        auto p_body = abst_body(p);
+        auto t_body = abst_body(t);
+        return match(p_domain, t_domain, o + 1, s);
+    }
 }
-bool fo_match::match_type(context & ctx, expr const & p, expr const & t, subst_map & s) {
-    std::cerr << "match_type : ("
-              << ctx << " ,"
-              << p << ", "
-              << t << ", "
-//              << s << ")"
-              << std::endl;
-    // TODO(soonho)
-    return true;
+
+bool fo_match::match_type(expr const & p, expr const & t, unsigned o, subst_map & s) {
+    cout << "match_type : ("
+         << p << ", "
+         << t << ", "
+         << o << ", "
+         << s << ")"
+         << endl;
+    return p == t;
 }
-bool fo_match::match_eq(context & ctx, expr const & p, expr const & t, subst_map & s) {
-    std::cerr << "match_eq : ("
-              << ctx << " ,"
-              << p << ", "
-              << t << ", "
-//              << s << ")"
-              << std::endl;
-    // TODO(soonho)
-    return true;
+
+bool fo_match::match_eq(expr const & p, expr const & t, unsigned o, subst_map & s) {
+    cout << "match_eq : ("
+         << p << ", "
+         << t << ", "
+         << o << ", "
+         << s << ")"
+         << endl;
+    return match(eq_lhs(p), eq_lhs(t), o, s) && match(eq_rhs(p), eq_rhs(t), o, s);
 }
-bool fo_match::match_let(context & ctx, expr const & p, expr const & t, subst_map & s) {
-    std::cerr << "match_let : ("
-              << ctx << " ,"
-              << p << ", "
-              << t << ", "
-//              << s << ")"
-              << std::endl;
-    // TODO(soonho)
-    return true;
+
+bool fo_match::match_let(expr const & p, expr const & t, unsigned o, subst_map & s) {
+    cout << "match_let : ("
+         << p << ", "
+         << t << ", "
+         << o << ", "
+         << s << ")"
+         << endl;
+
+    if (!is_let(t)) {
+        return false;
+    } else {
+        // First match the type part
+        auto p_type = let_type(p);
+        auto t_type = let_type(t);
+        if (!match(p_type, t_type, o, s))
+            return false;
+
+        // then match the value part
+        auto p_value = let_value(p);
+        auto t_value = let_value(t);
+        if (!match(p_value, t_value, o, s))
+            return false;
+
+        // then match the value part
+        auto p_body = let_body(p);
+        auto t_body = let_body(t);
+        return match(p_body, t_body, o + 1, s);
+    }
 }
-bool fo_match::match_metavar(context & ctx, expr const & p, expr const & t, subst_map & s) {
-    std::cerr << "match_meta : ("
-              << ctx << " ,"
-              << p << ", "
-              << t << ", "
-//              << s << ")"
-              << std::endl;
-    // TODO(soonho)
-    return true;
+bool fo_match::match_metavar(expr const & p, expr const & t, unsigned o, subst_map & s) {
+    cout << "match_meta : ("
+         << p << ", "
+         << t << ", "
+         << o << ", "
+         << s << ")"
+         << endl;
+    return p == t;
 }
-bool fo_match::match(context & ctx, expr const & p, expr const & t, subst_map & s) {
-    std::cerr << "match : ("
-              << ctx << " ,"
-              << p << ", "
-              << t << ", "
-//              << s << ")"
-              << std::endl;
+
+bool fo_match::match(expr const & p, expr const & t, unsigned o, subst_map & s) {
+    cout << "match : ("
+         << p << ", "
+         << t << ", "
+         << o << ", "
+         << s << ")"
+         << endl;
 
     switch (p.kind()) {
     case expr_kind::Var:
-        return match_var(ctx, p, t, s);
+        return match_var(p, t, o, s);
     case expr_kind::Constant:
-        return match_constant(ctx, p, t, s);
+        return match_constant(p, t, o, s);
     case expr_kind::Value:
-        return match_value(ctx, p, t, s);
+        return match_value(p, t, o, s);
     case expr_kind::App:
-        return match_app(ctx, p, t, s);
+        return match_app(p, t, o, s);
     case expr_kind::Lambda:
-        return match_lambda(ctx, p, t, s);
+        return match_lambda(p, t, o, s);
     case expr_kind::Pi:
-        return match_pi(ctx, p, t, s);
+        return match_pi(p, t, o, s);
     case expr_kind::Type:
-        return match_type(ctx, p, t, s);
+        return match_type(p, t, o, s);
     case expr_kind::Eq:
-        return match_eq(ctx, p, t, s);
+        return match_eq(p, t, o, s);
     case expr_kind::Let:
-        return match_let(ctx, p, t, s);
+        return match_let(p, t, o, s);
     case expr_kind::MetaVar:
-        return match_metavar(ctx, p, t, s);
+        return match_metavar(p, t, o, s);
     }
+    lean_unreachable();
+    return false;
 }
 }
diff --git a/src/library/rewrite/fo_match.h b/src/library/rewrite/fo_match.h
index b096f706d..3fd09934d 100644
--- a/src/library/rewrite/fo_match.h
+++ b/src/library/rewrite/fo_match.h
@@ -7,6 +7,7 @@ Author: Soonho Kong
 #pragma once
 #include <unordered_map>
 #include "kernel/expr.h"
+#include "kernel/expr_maps.h"
 #include "kernel/context.h"
 #include "util/list.h"
 #include "library/all/all.h"
@@ -16,21 +17,22 @@ Author: Soonho Kong
 
 namespace lean {
 
-typedef std::unordered_map<unsigned, expr const &> subst_map;
+typedef expr_map<expr> subst_map;
 
 class fo_match {
 private:
-    bool match_var(context & ctx, expr const & p, expr const & t, subst_map & s);
-    bool match_constant(context & ctx, expr const & p, expr const & t, subst_map & s);
-    bool match_value(context & ctx, expr const & p, expr const & t, subst_map & s);
-    bool match_app(context & ctx, expr const & p, expr const & t, subst_map & s);
-    bool match_lambda(context & ctx, expr const & p, expr const & t, subst_map & s);
-    bool match_pi(context & ctx, expr const & p, expr const & t, subst_map & s);
-    bool match_type(context & ctx, expr const & p, expr const & t, subst_map & s);
-    bool match_eq(context & ctx, expr const & p, expr const & t, subst_map & s);
-    bool match_let(context & ctx, expr const & p, expr const & t, subst_map & s);
-    bool match_metavar(context & ctx, expr const & p, expr const & t, subst_map & s);
+    bool match_var(expr const & p, expr const & t, unsigned o, subst_map & s);
+    bool match_constant(expr const & p, expr const & t, unsigned o, subst_map & s);
+    bool match_value(expr const & p, expr const & t, unsigned o, subst_map & s);
+    bool match_app(expr const & p, expr const & t, unsigned o, subst_map & s);
+    bool match_lambda(expr const & p, expr const & t, unsigned o, subst_map & s);
+    bool match_pi(expr const & p, expr const & t, unsigned o, subst_map & s);
+    bool match_type(expr const & p, expr const & t, unsigned o, subst_map & s);
+    bool match_eq(expr const & p, expr const & t, unsigned o, subst_map & s);
+    bool match_let(expr const & p, expr const & t, unsigned o, subst_map & s);
+    bool match_metavar(expr const & p, expr const & t, unsigned o, subst_map & s);
+
 public:
-    bool match(context & ctx, expr const & p, expr const & t, subst_map & s);
+    bool match(expr const & p, expr const & t, unsigned o, subst_map & s);
 };
 }
diff --git a/src/library/rewrite/rewrite.cpp b/src/library/rewrite/rewrite.cpp
index 20784fe20..fbe858121 100644
--- a/src/library/rewrite/rewrite.cpp
+++ b/src/library/rewrite/rewrite.cpp
@@ -4,14 +4,15 @@ Released under Apache 2.0 license as described in the file LICENSE.
 
 Author: Soonho Kong
 */
-#include "kernel/environment.h"
 #include "kernel/abstract.h"
 #include "kernel/builtin.h"
+#include "kernel/context.h"
+#include "kernel/environment.h"
+#include "library/printer.h"
+#include "library/rewrite/fo_match.h"
 #include "library/rewrite/rewrite.h"
 
 // Term Rewriting
-
-//
 // ORELSE
 // APP_RW
 // LAMBDA_RW
@@ -23,4 +24,54 @@ Author: Soonho Kong
 // FORALL
 // FAIL
 // FAIL_IF
-//
+
+using std::cout;
+using std::endl;
+
+namespace lean {
+
+theorem_rw::theorem_rw(expr const & t, expr const & v)
+    : thm_t(t), thm_v(v), num_args(0) {
+    cout << "================= Theorem Rewrite Constructor Start ===================" << endl;
+    cout << "Type = " << thm_t << endl;
+    cout << "Body = " << thm_v << endl;
+
+    // We expect the theorem is in the form of
+    // Pi (x_1 : t_1 ... x_n : t_n), t = s
+    expr tmp = t;
+    while (is_pi(tmp)) {
+        tmp = abst_body(tmp);
+        num_args++;
+    }
+    if (!is_eq(tmp)) {
+        cout << "Theorem " << t << " is not in the form of "
+                  << "Pi (x_1 : t_1 ... x_n : t_n), t = s" << endl;
+    }
+    cout << "OK. Number of Arg = " << num_args << endl;
+    cout << "================= Theorem Rewrite Constructor END ===================" << endl;
+}
+
+void theorem_rw::operator()(context & ctx, expr const & t) {
+    cout << "================= Theorem Rewrite () START ===================" << endl;
+    cout << "Context = " << ctx << endl;
+    cout << "Term = " << t << endl;
+    expr tmp = thm_t;
+    while (is_pi(tmp)) {
+        tmp = abst_body(tmp);
+        num_args++;
+    }
+    if (!is_eq(tmp)) {
+        cout << "Theorem " << t << " is not in the form of "
+                  << "Pi (x_1 : t_1 ... x_n : t_n), t = s" << endl;
+    }
+    expr const & lhs = eq_lhs(tmp);
+    expr const & rhs = eq_rhs(tmp);
+    fo_match fm;
+    subst_map s;
+    fm.match(lhs, t, 0, s);
+    // apply s to rhs
+
+    cout << "================= Theorem Rewrite () END ===================" << endl;
+}
+
+}
diff --git a/src/library/rewrite/rewrite.h b/src/library/rewrite/rewrite.h
index 4e48c2589..cb81d63f6 100644
--- a/src/library/rewrite/rewrite.h
+++ b/src/library/rewrite/rewrite.h
@@ -5,61 +5,17 @@ Released under Apache 2.0 license as described in the file LICENSE.
 Author: Soonho Kong
 */
 #pragma once
-#include <iostream>
-#include "kernel/context.h"
-#include "kernel/environment.h"
-#include "library/printer.h"
-#include "library/rewrite/fo_match.h"
 
 namespace lean {
 
-/**
-   \brief Format
-
-   uses `sexpr` as an internal representation.
-
-   nil                    = ["NIL"]
-   text         s         = ("TEXT"  . s)
-   choice       f1 f2     = ("CHOICE"  f1 . f2)
-   compose      f1 ... fn = ["COMPOSE" f1 ... fn]
-   line                   = ["LINE"]
-   nest         n  f      = ("NEST" n . f)
-   highlight    c  f      = ("HIGHLIGHT" c . f)
-*/
-
 class theorem_rw {
 private:
     expr const & thm_t;
     expr const & thm_v;
     unsigned num_args;
-public:
-    theorem_rw(expr const & t, expr const & v)
-        : thm_t(t), thm_v(v), num_args(0) {
-        std::cout << "Theorem_Rewrite "
-                  << "(" << thm_v << " : " << thm_t << ")"
-                  << std::endl;
-        // We expect the theorem is in the form of
-        // Pi (x_1 : t_1 ... x_n : t_n), t = s
-        expr tmp = t;
-        while (is_pi(tmp)) {
-            tmp = abst_body(tmp);
-            num_args++;
-        }
-        if (!is_eq(tmp)) {
-            std::cout << "Theorem " << t << " is not in the form of "
-                      << "Pi (x_1 : t_1 ... x_n : t_n), t = s" << std::endl;
-        }
-        std::cout << "Theorem " << t << " is OK. Number of Arg = " << num_args << std::endl;
-    }
 
-    void operator()(context & ctx, expr const & t) {
-        std::cout << "Theorem_Rewrite: ("
-                  << "Context : " << ctx
-                  << ", Term : " << t
-                  << ")" << std::endl;
-        fo_match fm;
-        subst_map s;
-        fm.match(ctx, thm_t, t, s);
-    }
+public:
+    theorem_rw(expr const & t, expr const & v);
+    void operator()(context & ctx, expr const & t);
 };
 }
diff --git a/src/tests/library/rewrite/rewrite.cpp b/src/tests/library/rewrite/rewrite.cpp
index c53e004c1..78dbd269d 100644
--- a/src/tests/library/rewrite/rewrite.cpp
+++ b/src/tests/library/rewrite/rewrite.cpp
@@ -12,6 +12,7 @@ Author: Soonho Kong
 #include "library/arith/nat.h"
 #include "library/rewrite/fo_match.h"
 #include "library/rewrite/rewrite.h"
+#include "library/printer.h"
 using namespace lean;
 
 using std::cout;
@@ -25,12 +26,17 @@ int main() {
     expr zero = nVal(0);                     // 0  : Nat
     expr x_plus_zero = nAdd(x, zero);        // x_plus_zero := x + 0
     expr y_plus_zero = nAdd(y, zero);        // y_plus_zero  := y + 0
-    std::cout << x_plus_zero << std::endl;
-    env.display(std::cout);
+    cout << "x := " << x << endl;
+    cout << "y := " << y << endl;
+    cout << "x + 0 := " << x_plus_zero << endl;
+    cout << "x + 0 := " << y_plus_zero << endl;
+    //env.display(cout);
 
     expr thm_t = Pi("x", Nat, Eq(nAdd(Const("x"), nVal(0)), Const("x"))); // Pi (x : Z), x + 0 = x
+    cout << "theorem := " << thm_t << endl;
     env.add_axiom("H1", thm_t); // H1 : Pi (x : N), x = x + 0
     expr thm_v = Const("H1");
+    cout << "axiom := " << thm_v << endl;
 
     theorem_rw trw(thm_t, thm_v);
     context ctx;