diff --git a/src/main/scala/leon/purescala/TypeTreeOps.scala b/src/main/scala/leon/purescala/TypeTreeOps.scala
index 6bdb5fa2493e8c0148a7fed4f228437569e48b1e..fc737493788fe49f9390f8d2034de86c6fdd5083 100644
--- a/src/main/scala/leon/purescala/TypeTreeOps.scala
+++ b/src/main/scala/leon/purescala/TypeTreeOps.scala
@@ -11,29 +11,75 @@ import Trees._
 import Extractors._
 
 object TypeTreeOps {
-  def canBeSubtypeOf(tpe: TypeTree, freeParams: Seq[TypeParameterDef], stpe: TypeTree): Option[Seq[TypeParameter]] = {
+  def canBeSubtypeOf(tpe: TypeTree, freeParams: Seq[TypeParameter], stpe: TypeTree): Option[Map[TypeParameter, TypeTree]] = {
+
+    def unify(res: Seq[Option[Map[TypeParameter, TypeTree]]]): Option[Map[TypeParameter, TypeTree]] = {
+      if (res.forall(_.isDefined)) {
+        var result = Map[TypeParameter, TypeTree]()
+
+        for (Some(m) <- res) {
+          result ++= m
+        }
+
+        Some(result)
+      } else {
+        None
+      }
+    }
+
     if (freeParams.isEmpty) {
       if (isSubtypeOf(tpe, stpe)) {
-        Some(Nil)
+        Some(Map())
       } else {
         None
       }
     } else {
-      // TODO
-      None
-    }
-  }
+      (tpe, stpe) match {
+        case (tp1: TypeParameter, t) =>
+          if (freeParams contains tp1) {
+            Some(Map(tp1 -> t))
+          } else if (tp1 == t) {
+            Some(Map())
+          } else {
+            None
+          }
 
-  def bestRealType(t: TypeTree) : TypeTree = t match {
-    case c: CaseClassType =>
-      c.classDef.parent match {
-        case None    =>
-          c
+        case (ct1: ClassType, ct2: ClassType) =>
+          val rt1 = ct1.root
+          val rt2 = ct2.root
+
+
+          if (rt1.classDef == rt2.classDef) {
+            unify((rt1.tps zip rt2.tps).map { case (tp1, tp2) =>
+              canBeSubtypeOf(tp1, freeParams, tp2)
+            })
+          } else {
+            None
+          }
+
+        case (_: TupleType, _: TupleType) |
+             (_: SetType, _: SetType) |
+             (_: MapType, _: MapType) |
+             (_: FunctionType, _: FunctionType) =>
+          val NAryType(ts1, _) = tpe
+          val NAryType(ts2, _) = stpe
 
-        case Some(p) =>
-          instantiateType(p, (c.classDef.tparams zip c.tps).toMap)
+          unify((ts1 zip ts2).map { case (tp1, tp2) =>
+            canBeSubtypeOf(tp1, freeParams, tp2)
+          })
+
+        case (t1, t2) =>
+          if (t1 == t2) {
+            Some(Map())
+          } else {
+            None
+          }
       }
+    }
+  }
 
+  def bestRealType(t: TypeTree) : TypeTree = t match {
+    case (c: CaseClassType) => c.root
     case NAryType(tps, builder) => builder(tps.map(bestRealType))
   }
 
diff --git a/src/main/scala/leon/purescala/TypeTrees.scala b/src/main/scala/leon/purescala/TypeTrees.scala
index 7e13555d0eca8e95e4e8e1a2c7f0832ecc28f432..9b518db9d357c02f6ac68b74b44497705681ec8d 100644
--- a/src/main/scala/leon/purescala/TypeTrees.scala
+++ b/src/main/scala/leon/purescala/TypeTrees.scala
@@ -151,6 +151,8 @@ object TypeTrees {
 
     lazy val fieldsTypes = fields.map(_.tpe)
 
+    lazy val root = parent.getOrElse(this)
+
     lazy val parent = classDef.parent.map {
       pct => instantiateType(pct, (classDef.tparams zip tps).toMap) match {
         case act: AbstractClassType  => act
diff --git a/src/main/scala/leon/synthesis/rules/Cegis.scala b/src/main/scala/leon/synthesis/rules/Cegis.scala
index 37d19bb3620144cbab60fd758cb3ef5f893eb163..7a6c7f0b943fa8e1ceed38e62d857dac8f767aa9 100644
--- a/src/main/scala/leon/synthesis/rules/Cegis.scala
+++ b/src/main/scala/leon/synthesis/rules/Cegis.scala
@@ -130,10 +130,12 @@ case object CEGIS extends Rule("CEGIS") {
               false
           }
 
+
           if (!isRecursiveCall && isNotSynthesizable) {
-            canBeSubtypeOf(fd.returnType, fd.tparams, t) match {
-              case Some(tps) =>
-                Some(fd.typed(tps))
+            val free = fd.tparams.map(_.tp)
+            canBeSubtypeOf(fd.returnType, free, t) match {
+              case Some(tpsMap) =>
+                Some(fd.typed(free.map(tp => tpsMap.getOrElse(tp, tp))))
               case None =>
                 None
             }