diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 8f260282512027eb361afa3f86f7b0cdb0fc75b6..c60988e12317074aee65e72688cc2648b1c61815 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,7 +1,7 @@
 # driver source files
-SET(sources imagine-planner.cpp types.cpp)
+SET(sources imagine-planner.cpp types.cpp queries.cpp)
 # application header files
-SET(headers imagine-planner.h types.h)
+SET(headers imagine-planner.h types.h queries.h)
 # Boost
 FIND_PACKAGE(Boost COMPONENTS system filesystem unit_test_framework REQUIRED)
 # Swi-pl
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 1d47022b3601cdacb4f575b9977a7b4c050e7aa5..27851e08db4d615d18d3b700109c0dfca5f87210 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -10,4 +10,10 @@ TARGET_LINK_LIBRARIES(types_test
                       ${Boost_SYSTEM_LIBRARY}
                       ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY})
 
+ADD_EXECUTABLE(queries_test queries_test.cpp)
+TARGET_LINK_LIBRARIES(queries_test
+                      imagine-planner
+                      ${Boost_FILESYSTEM_LIBRARY}
+                      ${Boost_SYSTEM_LIBRARY}
+                      ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY})
 
diff --git a/src/examples/queries_test.cpp b/src/examples/queries_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e520790997570e3e0f7d8e42b4159e015140c6d9
--- /dev/null
+++ b/src/examples/queries_test.cpp
@@ -0,0 +1,47 @@
+#define BOOST_TEST_MODULE Types Test
+#define BOOST_TEST_DYN_LINK
+#include <boost/test/unit_test.hpp>
+
+#include "queries.h"
+#include <iostream>
+using namespace imagine_planner;
+
+#if 0 // Set to 1 if __PRETTY_FUNCTION__ macro is not available
+#define __PRETTY_FUNCTION__ __func__
+#endif
+
+#if 1
+#define INFO(x)\
+  std::cout << "\e[1m\e[32m\e[4m" <<  __PRETTY_FUNCTION__ << " [" << __LINE__ << "]" << "\e[24m: " << x << "\e[0m" << std::endl;
+#else
+#define INFO(x)
+#endif
+
+BOOST_AUTO_TEST_CASE(simple_query_test_sat)
+{
+  PlanState state({Predicate("p","a","b"), Predicate("p","a","c"),
+      Predicate("p","d","a"), Predicate("q","w"), Predicate("q", "s")});
+  INFO(state);
+  SimpleQuery q1(state, Predicate("p", "a", "X"));
+  SimpleQuery q2(state, Predicate("q", "w"));
+  BOOST_REQUIRE(q1.next_solution());
+  BOOST_CHECK_EQUAL(q1.get_solution().to_str(), "{\n  X -> b\n}");
+  BOOST_REQUIRE(q1.next_solution());
+  BOOST_CHECK_EQUAL(q1.get_solution().to_str(), "{\n  X -> c\n}");
+  BOOST_REQUIRE(not q1.next_solution());
+  BOOST_REQUIRE(q2.next_solution());
+  BOOST_CHECK_EQUAL(q2.get_solution().to_str(), "{}");
+  BOOST_CHECK(not q1.next_solution());
+}
+
+BOOST_AUTO_TEST_CASE(simple_query_test_unsat)
+{
+  PlanState state({Predicate("p","a","b"), Predicate("p","a","c"),
+      Predicate("p","d","a"), Predicate("q","w"), Predicate("q", "s")});
+  INFO(state);
+  SimpleQuery q1(state, Predicate("p", "c", "X"));
+  SimpleQuery q2(state, Predicate("p", "a", "d"));
+  BOOST_REQUIRE(not q1.next_solution());
+  BOOST_REQUIRE(not q2.next_solution());
+}
+
diff --git a/src/examples/types_test.cpp b/src/examples/types_test.cpp
index f2d8c65c0d7100d2e1317c9de806cf37e21997c2..e220a3be4ef6ea33c1116118b156d6f63a491be6 100644
--- a/src/examples/types_test.cpp
+++ b/src/examples/types_test.cpp
@@ -241,6 +241,20 @@ BOOST_AUTO_TEST_CASE(substitution_test)
 BOOST_AUTO_TEST_CASE(planstate_test)
 {
   PlanState state({Predicate("p", "x", "y"), Predicate("q", "y"), Predicate("r")});
-  INFO(state);
+  PlanState state_(state), state__(state);
+  PlanState state2(state);
+  state__.remove(Predicate("p", "x", "y"));
+  state2.put(Predicate("q", "z"));
+  INFO("state: " << state << " (" << state.hash() << ')');
+  INFO("state_: " << state_ << " (" << state_.hash() << ')');
+  INFO("state__: " << state__ << " (" << state__.hash() << ')');
+  INFO("state2: " << state2 << " (" << state2.hash() << ')');
+  BOOST_CHECK_EQUAL(state, state_);
+  BOOST_CHECK_NE(state, state__);
+  BOOST_CHECK_NE(state, state2);
+  BOOST_CHECK(state.subset_of(state2));
+  BOOST_CHECK(state__.subset_of(state));
+  BOOST_CHECK(not state.subset_of(state__));
+  BOOST_CHECK(not state2.subset_of(state));
 }
 
diff --git a/src/queries.cpp b/src/queries.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6ddecd77f9354809c0f443d5fc3c59078d8b793
--- /dev/null
+++ b/src/queries.cpp
@@ -0,0 +1,58 @@
+#include "queries.h"
+
+namespace imagine_planner
+{
+
+bool SimpleQuery::unify(const Predicate& goal)
+{
+  assignment_.clear();
+  if (goal.arity() != target_.arity()) return false;
+  if (goal.get_name() != target_.get_name()) return false;
+  for (int idx = 0; idx < goal.arity(); ++idx)
+  {
+    const TermWrapper& term1 = target_.get_arguments()[idx];
+    const TermWrapper& term2 = goal.get_arguments()[idx];
+    if (term1.is_ground())
+    {
+      if (term1 != term2) return false;
+    }
+    else if (const Term* assig = assignment_.get(term1.to_str()))
+    {
+      if (*assig != *term2.get_term()) return false;
+    }
+    else
+    {
+      assignment_.put(term1.to_str(), term2);
+    }
+  }
+  return true;
+}
+
+SimpleQuery::SimpleQuery(const PlanState& state, const Predicate& target) :
+  state_(state), target_(target)
+{
+  cursor_ = state_.begin();
+}
+
+bool SimpleQuery::next_solution()
+{
+  if (cursor_ != state_.end() and target_.is_ground())
+  {
+    cursor_ = state_.end();
+    return state_.has(target_);
+  }
+
+  while (cursor_ != state_.end())
+  {
+    if (unify(*cursor_))
+    {
+      ++cursor_;
+      return true;
+    }
+    ++cursor_;
+  }
+  return false;
+}
+
+}
+
diff --git a/src/queries.h b/src/queries.h
new file mode 100644
index 0000000000000000000000000000000000000000..2750220500817d45c3f9a3b1463958b39c829783
--- /dev/null
+++ b/src/queries.h
@@ -0,0 +1,47 @@
+#ifndef _LIBIMAGINE_PLANNER_QUERIES_H_
+#define _LIBIMAGINE_PLANNER_QUERIES_H_
+
+#include "types.h"
+
+namespace imagine_planner
+{
+
+class Query
+{
+  public:
+    virtual ~Query() {};
+
+    virtual bool next_solution() =0;
+
+    virtual const Substitution& get_solution() const=0;
+};
+
+class SimpleQuery : public Query
+{
+  private:
+    const PlanState& state_;
+    Predicate target_;
+    Substitution assignment_;
+    PlanState::CIter cursor_;
+
+    bool unify(const Predicate& goal);
+    
+  public:
+    SimpleQuery(const PlanState& state, const Predicate& target);
+
+    virtual bool next_solution() override;
+
+    const Substitution& get_solution() const { return assignment_; }
+
+};
+
+class NotQuery : public Query
+{
+  private:
+    Query q_;
+};
+
+} /* end namespace imagine-planner */
+
+#endif
+
diff --git a/src/types.cpp b/src/types.cpp
index 7ed05af7be6d5121b96beff437ccbef93c372a54..704e501ae6cb903faddeaedc979a00b08a37b79f 100644
--- a/src/types.cpp
+++ b/src/types.cpp
@@ -109,6 +109,8 @@ TermWrapper::~TermWrapper()
   delete term_;
 }
 
+TermV create_term_v() { return TermV(); }
+
 // Predicate
 
 Predicate::Predicate(const std::string& name, const TermV& arguments) :
@@ -306,7 +308,6 @@ std::size_t PlanState::hash() const
   return acc;
 }
 
-
 bool PlanState::subset_of(const PlanState& other) const
 {
   if (predicates_.size() > other.predicates_.size()) return false;
diff --git a/src/types.h b/src/types.h
index ce5a13a79911a0e8f47543bbe34dd555a80e0075..24b9f45d8076ab6b543b0782e8185f3e24e7b12d 100644
--- a/src/types.h
+++ b/src/types.h
@@ -191,7 +191,7 @@ class TermWrapper : public Stringifiable,
 
 typedef std::vector<TermWrapper> TermV;
 
-TermV create_term_v() { return TermV(); }
+TermV create_term_v();
 
 template <typename First, typename... Args>
 TermV create_term_v(const First& fst, Args... args)
@@ -261,6 +261,8 @@ class Substitution : public Stringifiable
 
     void remove(const std::string& varname);
 
+    void clear() { sigma_.clear(); }
+
     void operator+=(const Substitution& other);
 
     void operator-=(const Substitution& other);
@@ -355,6 +357,6 @@ struct hash<imagine_planner::Hashable>
   std::size_t operator()(const imagine_planner::Hashable& hashable) const;
 };
 
-}
+} /* end std namespace */
 
 #endif