diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp
index dbf1e93f9..19e46fd85 100644
--- a/wrap/Argument.cpp
+++ b/wrap/Argument.cpp
@@ -29,24 +29,21 @@ using namespace std;
 using namespace wrap;
 
 /* ************************************************************************* */
-Argument Argument::expandTemplate(const string& templateArg,
-    const Qualified& qualifiedType, const Qualified& expandedClass) const {
+Argument Argument::expandTemplate(const TemplateSubstitution& ts) const {
   Argument instArg = *this;
-  if (type.name == templateArg) {
-    instArg.type = qualifiedType;
+  if (type.name == ts.templateArg) {
+    instArg.type = ts.qualifiedType;
   } else if (type.name == "This") {
-    instArg.type = expandedClass;
+    instArg.type = ts.expandedClass;
   }
   return instArg;
 }
 
 /* ************************************************************************* */
-ArgumentList ArgumentList::expandTemplate(const string& templateArg,
-    const Qualified& qualifiedType, const Qualified& expandedClass) const {
+ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const {
   ArgumentList instArgList;
   BOOST_FOREACH(const Argument& arg, *this) {
-    Argument instArg = arg.expandTemplate(templateArg, qualifiedType,
-        expandedClass);
+    Argument instArg = arg.expandTemplate(ts);
     instArgList.push_back(instArg);
   }
   return instArgList;
diff --git a/wrap/Argument.h b/wrap/Argument.h
index 5fba1daef..5a4f08a25 100644
--- a/wrap/Argument.h
+++ b/wrap/Argument.h
@@ -19,7 +19,7 @@
 
 #pragma once
 
-#include "Qualified.h"
+#include "TemplateSubstitution.h"
 #include "FileWriter.h"
 #include "ReturnValue.h"
 
@@ -35,8 +35,7 @@ struct Argument {
       is_const(false), is_ref(false), is_ptr(false) {
   }
 
-  Argument expandTemplate(const std::string& templateArg,
-      const Qualified& qualifiedType, const Qualified& expandedClass) const;
+  Argument expandTemplate(const TemplateSubstitution& ts) const;
 
   /// return MATLAB class for use in isa(x,class)
   std::string matlabClass(const std::string& delim = "") const;
@@ -63,8 +62,7 @@ struct ArgumentList: public std::vector<Argument> {
   /// Check if all arguments scalar
   bool allScalar() const;
 
-  ArgumentList expandTemplate(const std::string& templateArg,
-      const Qualified& qualifiedType, const Qualified& expandedClass) const;
+  ArgumentList expandTemplate(const TemplateSubstitution& ts) const;
 
   // MATLAB code generation:
 
@@ -110,14 +108,7 @@ inline void verifyArguments(const std::vector<std::string>& validArgs,
   typedef typename std::map<std::string, T>::value_type NamedMethod;
   BOOST_FOREACH(const NamedMethod& namedMethod, vt) {
     const T& t = namedMethod.second;
-    BOOST_FOREACH(const ArgumentList& argList, t.argLists) {
-      BOOST_FOREACH(Argument arg, argList) {
-        std::string fullType = arg.type.qualifiedName("::");
-        if (find(validArgs.begin(), validArgs.end(), fullType)
-            == validArgs.end())
-          throw DependencyMissing(fullType, t.name_);
-      }
-    }
+    t.verifyArguments(validArgs,t.name_);
   }
 }
 
diff --git a/wrap/Class.cpp b/wrap/Class.cpp
index e7dca4ace..d219452a3 100644
--- a/wrap/Class.cpp
+++ b/wrap/Class.cpp
@@ -240,15 +240,11 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile,
 }
 
 /* ************************************************************************* */
-Class Class::expandTemplate(const string& templateArg,
-    const Qualified& instName, const Qualified& expandedClass) const {
+Class Class::expandTemplate(const TemplateSubstitution& ts) const {
   Class inst = *this;
-  inst.methods = expandMethodTemplate(methods, templateArg, instName,
-      expandedClass);
-  inst.static_methods = expandMethodTemplate(static_methods, templateArg,
-      instName, expandedClass);
-  inst.constructor.args_list = inst.constructor.expandArgumentListsTemplate(
-      templateArg, instName, expandedClass);
+  inst.methods = expandMethodTemplate(methods, ts);
+  inst.static_methods = expandMethodTemplate(static_methods, ts);
+  inst.constructor.args_list = inst.constructor.expandArgumentListsTemplate(ts);
   inst.constructor.name = inst.name;
   inst.deconstructor.name = inst.name;
   return inst;
@@ -261,7 +257,8 @@ vector<Class> Class::expandTemplate(const string& templateArg,
   BOOST_FOREACH(const Qualified& instName, instantiations) {
     Qualified expandedClass = (Qualified) (*this);
     expandedClass.name += instName.name;
-    Class inst = expandTemplate(templateArg, instName, expandedClass);
+    const TemplateSubstitution ts(templateArg, instName, expandedClass);
+    Class inst = expandTemplate(ts);
     inst.name = expandedClass.name;
     inst.templateArgs.clear();
     inst.typedefName = qualifiedName("::") + "<" + instName.qualifiedName("::")
@@ -282,13 +279,12 @@ void Class::addMethod(bool verbose, bool is_const, const string& name,
     BOOST_FOREACH(const Qualified& instName, templateArgValues) {
       string expandedName = name + instName.name;
       // substitute template in arguments
-      ArgumentList expandedArgs = args.expandTemplate(templateArgName, instName,
-          name);
+      const TemplateSubstitution ts(templateArgName, instName, name);
+      ArgumentList expandedArgs = args.expandTemplate(ts);
       // do the same for return types
-      ReturnValue expandedRetVal = retVal.expandTemplate(templateArgName,
-          instName, name);
-      methods[expandedName].addOverload(verbose, is_const, expandedName,
-          expandedArgs, expandedRetVal, instName);
+      ReturnValue expandedRetVal = retVal.expandTemplate(ts);
+      methods[expandedName].addOverload(verbose, is_const, name, expandedArgs,
+          expandedRetVal, instName);
     }
   } else
     // just add overload
@@ -390,24 +386,14 @@ void Class::comment_fragment(FileWriter& proxyFile) const {
     proxyFile.oss << "%\n%-------Methods-------\n";
   BOOST_FOREACH(const Methods::value_type& name_m, methods) {
     const Method& m = name_m.second;
-    BOOST_FOREACH(ArgumentList argList, m.argLists) {
-      proxyFile.oss << "%";
-      argList.emit_prototype(proxyFile, m.name_);
-      proxyFile.oss << " : returns " << m.returnVals[0].return_type(false)
-          << endl;
-    }
+    m.comment_fragment(proxyFile, m.name_);
   }
 
   if (!static_methods.empty())
     proxyFile.oss << "%\n%-------Static Methods-------\n";
   BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) {
     const StaticMethod& m = name_m.second;
-    BOOST_FOREACH(ArgumentList argList, m.argLists) {
-      proxyFile.oss << "%";
-      argList.emit_prototype(proxyFile, m.name_);
-      proxyFile.oss << " : returns " << m.returnVals[0].return_type(false)
-          << endl;
-    }
+    m.comment_fragment(proxyFile, m.name_);
   }
 
   if (hasSerialization) {
diff --git a/wrap/Class.h b/wrap/Class.h
index 9422482b4..610c9b7b4 100644
--- a/wrap/Class.h
+++ b/wrap/Class.h
@@ -67,8 +67,7 @@ public:
       const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
       FileWriter& wrapperFile, std::vector<std::string>& functionNames) const; ///< emit proxy class
 
-  Class expandTemplate(const std::string& templateArg,
-      const Qualified& instantiation, const Qualified& expandedClass) const;
+  Class expandTemplate(const TemplateSubstitution& ts) const;
 
   std::vector<Class> expandTemplate(const std::string& templateArg,
       const std::vector<Qualified>& instantiations) const;
diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp
index a44f0893d..98a689ced 100644
--- a/wrap/Constructor.cpp
+++ b/wrap/Constructor.cpp
@@ -38,12 +38,10 @@ string Constructor::matlab_wrapper_name(const string& className) const {
 
 /* ************************************************************************* */
 vector<ArgumentList> Constructor::expandArgumentListsTemplate(
-    const string& templateArg, const Qualified& qualifiedType,
-    const Qualified& expandedClass) const {
+    const TemplateSubstitution& ts) const {
   vector<ArgumentList> result;
   BOOST_FOREACH(const ArgumentList& argList, args_list) {
-    ArgumentList instArgList = argList.expandTemplate(templateArg,
-        qualifiedType, expandedClass);
+    ArgumentList instArgList = argList.expandTemplate(ts);
     result.push_back(instArgList);
   }
   return result;
diff --git a/wrap/Constructor.h b/wrap/Constructor.h
index 49a731a7d..40bca549a 100644
--- a/wrap/Constructor.h
+++ b/wrap/Constructor.h
@@ -40,8 +40,7 @@ struct Constructor {
 
   // TODO eliminate copy/paste with function
   std::vector<ArgumentList> expandArgumentListsTemplate(
-      const std::string& templateArg, const Qualified& qualifiedType,
-      const Qualified& expandedClass) const;
+      const TemplateSubstitution& ts) const;
 
   // MATLAB code generation
   // toolboxPath is main toolbox directory, e.g., ../matlab
diff --git a/wrap/Function.cpp b/wrap/Function.cpp
index 8fd1d0655..ab3958c62 100644
--- a/wrap/Function.cpp
+++ b/wrap/Function.cpp
@@ -30,7 +30,6 @@ using namespace wrap;
 
 /* ************************************************************************* */
 void Function::addOverload(bool verbose, const std::string& name,
-    const ArgumentList& args, const ReturnValue& retVal,
     const Qualified& instName) {
 
   // Check if this overload is give to the correct method
@@ -51,18 +50,14 @@ void Function::addOverload(bool verbose, const std::string& name,
             + templateArgValue_.qualifiedName(":"));
 
   verbose_ = verbose;
-  argLists.push_back(args);
-  returnVals.push_back(retVal);
 }
 
 /* ************************************************************************* */
-vector<ArgumentList> Function::expandArgumentListsTemplate(
-    const string& templateArg, const Qualified& qualifiedType,
-    const Qualified& expandedClass) const {
+vector<ArgumentList> ArgumentOverloads::expandArgumentListsTemplate(
+    const TemplateSubstitution& ts) const {
   vector<ArgumentList> result;
-  BOOST_FOREACH(const ArgumentList& argList, argLists) {
-    ArgumentList instArgList = argList.expandTemplate(templateArg,
-        qualifiedType, expandedClass);
+  BOOST_FOREACH(const ArgumentList& argList, argLists_) {
+    ArgumentList instArgList = argList.expandTemplate(ts);
     result.push_back(instArgList);
   }
   return result;
diff --git a/wrap/Function.h b/wrap/Function.h
index 76b513907..dd6d2158c 100644
--- a/wrap/Function.h
+++ b/wrap/Function.h
@@ -42,66 +42,163 @@ struct Function {
   bool verbose_;
   std::string name_; ///< name of method
   Qualified templateArgValue_; ///< value of template argument if applicable
-  std::vector<ArgumentList> argLists;
-  std::vector<ReturnValue> returnVals;
 
   // The first time this function is called, it initializes the class members
   // with those in rhs, but in subsequent calls it adds additional argument
   // lists as function overloads.
   void addOverload(bool verbose, const std::string& name,
-      const ArgumentList& args, const ReturnValue& retVal,
       const Qualified& instName = Qualified());
+};
+
+/**
+ * ArgumentList Overloads
+ */
+class ArgumentOverloads {
+
+protected:
+
+  std::vector<ArgumentList> argLists_;
+
+public:
+
+  size_t nrOverloads() const {
+    return argLists_.size();
+  }
+
+  const ArgumentList& argumentList(size_t i) const {
+    return argLists_.at(i);
+  }
+
+  void addOverload(const ArgumentList& args) {
+    argLists_.push_back(args);
+  }
 
   std::vector<ArgumentList> expandArgumentListsTemplate(
-      const std::string& templateArg, const Qualified& qualifiedType,
-      const Qualified& expandedClass) const;
+      const TemplateSubstitution& ts) const;
+
+  /// Expand templates, imperative !
+  virtual void ExpandTemplate(const TemplateSubstitution& ts) {
+    argLists_ = expandArgumentListsTemplate(ts);
+  }
+
+  void verifyArguments(const std::vector<std::string>& validArgs,
+      const std::string s) const {
+    BOOST_FOREACH(const ArgumentList& argList, argLists_) {
+      BOOST_FOREACH(Argument arg, argList) {
+        std::string fullType = arg.type.qualifiedName("::");
+        if (find(validArgs.begin(), validArgs.end(), fullType)
+            == validArgs.end())
+          throw DependencyMissing(fullType, s);
+      }
+    }
+  }
+
+};
+
+/**
+ * Signature Overload (including return value)
+ */
+class SignatureOverloads: public ArgumentOverloads {
+
+protected:
+
+  std::vector<ReturnValue> returnVals_;
+
+public:
+
+  const ReturnValue& returnValue(size_t i) const {
+    return returnVals_.at(i);
+  }
+
+  void addOverload(const ArgumentList& args, const ReturnValue& retVal) {
+    argLists_.push_back(args);
+    returnVals_.push_back(retVal);
+  }
+
+  void verifyReturnTypes(const std::vector<std::string>& validtypes,
+      const std::string& s) const {
+    BOOST_FOREACH(const ReturnValue& retval, returnVals_) {
+      retval.type1.verify(validtypes, s);
+      if (retval.isPair)
+        retval.type2.verify(validtypes, s);
+    }
+  }
+
+  // TODO use transform ?
+  std::vector<ReturnValue> ExpandReturnValuesTemplate(
+      const TemplateSubstitution& ts) const {
+    std::vector<ReturnValue> result;
+    BOOST_FOREACH(const ReturnValue& retVal, returnVals_) {
+      ReturnValue instRetVal = retVal.expandTemplate(ts);
+      result.push_back(instRetVal);
+    }
+    return result;
+  }
+
+  /// Expand templates, imperative !
+  void expandTemplate(const TemplateSubstitution& ts) {
+    // substitute template in arguments
+    argLists_ = expandArgumentListsTemplate(ts);
+    // do the same for return types
+    returnVals_ = ExpandReturnValuesTemplate(ts);
+  }
+
+  // emit a list of comments, one for each overload
+  void usage_fragment(FileWriter& proxyFile, const std::string& name) const {
+    unsigned int argLCount = 0;
+    BOOST_FOREACH(ArgumentList argList, argLists_) {
+      argList.emit_prototype(proxyFile, name);
+      if (argLCount != nrOverloads() - 1)
+        proxyFile.oss << ", ";
+      else
+        proxyFile.oss << " : returns " << returnValue(0).return_type(false)
+            << std::endl;
+      argLCount++;
+    }
+  }
+
+  // emit a list of comments, one for each overload
+  void comment_fragment(FileWriter& proxyFile, const std::string& name) const {
+    size_t i = 0;
+    BOOST_FOREACH(ArgumentList argList, argLists_) {
+      proxyFile.oss << "%";
+      argList.emit_prototype(proxyFile, name);
+      proxyFile.oss << " : returns " << returnVals_[i++].return_type(false)
+          << std::endl;
+    }
+  }
+
 };
 
 // Templated checking functions
 // TODO: do this via polymorphism ?
 
-template<class FUNCTION>
-FUNCTION expandMethodTemplate(const FUNCTION& method,
-    const std::string& templateArg, const Qualified& qualifiedType,
-    const Qualified& expandedClass) {
-  // Create new instance
-  FUNCTION instMethod = method;
-  // substitute template in arguments
-  instMethod.argLists = method.expandArgumentListsTemplate(templateArg,
-      qualifiedType, expandedClass);
-  // do the same for return types
-  instMethod.returnVals = ReturnValue::ExpandTemplate(method.returnVals,
-      templateArg, qualifiedType, expandedClass);
-  // return new method
+template<class F>
+F expandMethodTemplate(F& method, const TemplateSubstitution& ts) {
+  F instMethod = method;
+  method.expandTemplate(ts);
   return instMethod;
 }
 
 // TODO use transform
-template<class FUNCTION>
-static std::map<std::string, FUNCTION> expandMethodTemplate(
-    const std::map<std::string, FUNCTION>& methods,
-    const std::string& templateArg, const Qualified& qualifiedType,
-    const Qualified& expandedClass) {
-  std::map<std::string, FUNCTION> result;
-  typedef std::pair<const std::string, FUNCTION> NamedMethod;
+template<class F>
+static std::map<std::string, F> expandMethodTemplate(
+    const std::map<std::string, F>& methods, const TemplateSubstitution& ts) {
+  std::map<std::string, F> result;
+  typedef std::pair<const std::string, F> NamedMethod;
   BOOST_FOREACH(NamedMethod namedMethod, methods) {
-    namedMethod.second = expandMethodTemplate(namedMethod.second, templateArg,
-        qualifiedType, expandedClass);
+    namedMethod.second = expandMethodTemplate(namedMethod.second, ts);
     result.insert(namedMethod);
   }
   return result;
 }
-template<class T>
-inline void verifyReturnTypes(const std::vector<std::string>& validtypes,
-    const std::map<std::string, T>& vt) {
-  typedef typename std::map<std::string, T>::value_type NamedMethod;
+template<class F>
+inline void verifyReturnTypes(const std::vector<std::string>& validTypes,
+    const std::map<std::string, F>& vt) {
+  typedef typename std::map<std::string, F>::value_type NamedMethod;
   BOOST_FOREACH(const NamedMethod& namedMethod, vt) {
-    const T& t = namedMethod.second;
-    BOOST_FOREACH(const ReturnValue& retval, t.returnVals) {
-      retval.type1.verify(validtypes, t.name_);
-      if (retval.isPair)
-        retval.type2.verify(validtypes, t.name_);
-    }
+    const F& t = namedMethod.second;
+    t.verifyReturnTypes(validTypes, t.name_);
   }
 }
 
diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp
index 05b954652..1f9d6518e 100644
--- a/wrap/GlobalFunction.cpp
+++ b/wrap/GlobalFunction.cpp
@@ -19,7 +19,8 @@ using namespace std;
 void GlobalFunction::addOverload(bool verbose, const Qualified& overload,
     const ArgumentList& args, const ReturnValue& retVal,
     const Qualified& instName) {
-  Function::addOverload(verbose, overload.name, args, retVal, instName);
+  Function::addOverload(verbose, overload.name, instName);
+  SignatureOverloads::addOverload(args, retVal);
   overloads.push_back(overload);
 }
 
@@ -37,15 +38,10 @@ void GlobalFunction::matlab_proxy(const std::string& toolboxPath,
     Qualified overload = overloads.at(i);
     // use concatenated namespaces as key
     string str_ns = qualifiedName("", overload.namespaces);
-    ReturnValue ret = returnVals.at(i);
-    ArgumentList args = argLists.at(i);
-
-    if (!grouped_functions.count(str_ns))
-      grouped_functions[str_ns] = GlobalFunction(name_, verbose_);
-
-    grouped_functions[str_ns].argLists.push_back(args);
-    grouped_functions[str_ns].returnVals.push_back(ret);
-    grouped_functions[str_ns].overloads.push_back(overload);
+    const ReturnValue& ret = returnValue(i);
+    const ArgumentList& args = argumentList(i);
+    grouped_functions[str_ns].addOverload(verbose_, overload, args, ret,
+        templateArgValue_);
   }
 
   size_t lastcheck = grouped_functions.size();
@@ -77,16 +73,15 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath,
 
   mfunctionFile.oss << "function varargout = " << name_ << "(varargin)\n";
 
-  for (size_t overload = 0; overload < argLists.size(); ++overload) {
-    const ArgumentList& args = argLists[overload];
-    const ReturnValue& returnVal = returnVals[overload];
+  for (size_t i = 0; i < nrOverloads(); ++i) {
+    const ArgumentList& args = argumentList(i);
+    const ReturnValue& returnVal = returnValue(i);
 
     const int id = functionNames.size();
 
     // Output proxy matlab code
-    mfunctionFile.oss << "      " << (overload == 0 ? "" : "else");
-    argLists[overload].emit_conditional_call(mfunctionFile,
-        returnVals[overload], wrapperName, id, true); // true omits "this"
+    mfunctionFile.oss << "      " << (i == 0 ? "" : "else");
+    args.emit_conditional_call(mfunctionFile, returnVal, wrapperName, id, true); // true omits "this"
 
     // Output C++ wrapper code
 
diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h
index 17d89d6f5..6f8686925 100644
--- a/wrap/GlobalFunction.h
+++ b/wrap/GlobalFunction.h
@@ -13,7 +13,7 @@
 
 namespace wrap {
 
-struct GlobalFunction: public Function {
+struct GlobalFunction: public Function, public SignatureOverloads {
 
   std::vector<Qualified> overloads; ///< Stack of qualified names
 
diff --git a/wrap/Method.cpp b/wrap/Method.cpp
index e218b45ec..d342df04b 100644
--- a/wrap/Method.cpp
+++ b/wrap/Method.cpp
@@ -29,123 +29,24 @@ using namespace std;
 using namespace wrap;
 
 /* ************************************************************************* */
-void Method::addOverload(bool verbose, bool is_const, const std::string& name_,
+void Method::addOverload(bool verbose, bool is_const, const std::string& name,
     const ArgumentList& args, const ReturnValue& retVal,
     const Qualified& instName) {
 
-  Function::addOverload(verbose, name_, args, retVal);
+  StaticMethod::addOverload(verbose, name, args, retVal, instName);
   is_const_ = is_const;
 }
 
 /* ************************************************************************* */
-void Method::proxy_wrapper_fragments(FileWriter& proxyFile,
-    FileWriter& wrapperFile, const string& cppClassName,
-    const std::string& matlabQualName, const std::string& matlabUniqueName,
-    const string& wrapperName, const TypeAttributesTable& typeAttributes,
-    vector<string>& functionNames) const {
-
-  // Create function header
+void Method::proxy_header(FileWriter& proxyFile) const {
   proxyFile.oss << "    function varargout = " << name_ << "(this, varargin)\n";
-
-  // Emit comments for documentation
-  string up_name = boost::to_upper_copy(name_);
-  proxyFile.oss << "      % " << up_name << " usage: ";
-  unsigned int argLCount = 0;
-  BOOST_FOREACH(ArgumentList argList, argLists) {
-    argList.emit_prototype(proxyFile, name_);
-    if (argLCount != argLists.size() - 1)
-      proxyFile.oss << ", ";
-    else
-      proxyFile.oss << " : returns " << returnVals[0].return_type(false)
-          << endl;
-    argLCount++;
-  }
-
-  // Emit URL to Doxygen page
-  proxyFile.oss << "      % "
-      << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html"
-      << endl;
-
-  // Document all overloads, if any
-  if (argLists.size() > 1) {
-    proxyFile.oss << "      % " << "" << endl;
-    proxyFile.oss << "      % " << "Method Overloads" << endl;
-    BOOST_FOREACH(ArgumentList argList, argLists) {
-      proxyFile.oss << "      % ";
-      argList.emit_prototype(proxyFile, name_);
-      proxyFile.oss << endl;
-    }
-  }
-
-  // Handle special case of single overload with all numeric arguments
-  if (argLists.size() == 1 && argLists[0].allScalar()) {
-    // Output proxy matlab code
-    proxyFile.oss << "      ";
-    const int id = (int) functionNames.size();
-    argLists[0].emit_call(proxyFile, returnVals[0], wrapperName, id);
-
-    // Output C++ wrapper code
-    const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName,
-        matlabUniqueName, 0, id, typeAttributes, templateArgValue_);
-
-    // Add to function list
-    functionNames.push_back(wrapFunctionName);
-  } else {
-    // Check arguments for all overloads
-    for (size_t overload = 0; overload < argLists.size(); ++overload) {
-
-      // Output proxy matlab code
-      proxyFile.oss << "      " << (overload == 0 ? "" : "else");
-      const int id = (int) functionNames.size();
-      string expanded = wrapperName;
-      if (!templateArgValue_.empty())
-        expanded += templateArgValue_.name;
-      argLists[overload].emit_conditional_call(proxyFile, returnVals[overload],
-          expanded, id);
-
-      // Output C++ wrapper code
-      const string wrapFunctionName = wrapper_fragment(wrapperFile,
-          cppClassName, matlabUniqueName, overload, id, typeAttributes,
-          templateArgValue_);
-
-      // Add to function list
-      functionNames.push_back(wrapFunctionName);
-    }
-    proxyFile.oss << "      else\n";
-    proxyFile.oss
-        << "        error('Arguments do not match any overload of function "
-        << matlabQualName << "." << name_ << "');" << endl;
-    proxyFile.oss << "      end\n";
-  }
-
-  proxyFile.oss << "    end\n";
 }
 
 /* ************************************************************************* */
-string Method::wrapper_fragment(FileWriter& wrapperFile,
-    const string& cppClassName, const string& matlabUniqueName, int overload,
-    int id, const TypeAttributesTable& typeAttributes,
+string Method::wrapper_call(FileWriter& wrapperFile, const string& cppClassName,
+    const string& matlabUniqueName, const ArgumentList& args,
+    const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
     const Qualified& instName) const {
-
-  // generate code
-
-  const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_"
-      + boost::lexical_cast<string>(id);
-
-  const ArgumentList& args = argLists[overload];
-  const ReturnValue& returnVal = returnVals[overload];
-
-  // call
-  wrapperFile.oss << "void " << wrapFunctionName
-      << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
-  // start
-  wrapperFile.oss << "{\n";
-
-  returnVal.wrapTypeUnwrap(wrapperFile);
-
-  wrapperFile.oss << "  typedef boost::shared_ptr<" << cppClassName
-      << "> Shared;" << endl;
-
   // check arguments
   // extra argument obj -> nargin-1 is passed !
   // example: checkArguments("equals",nargout,nargin-1,2);
@@ -156,24 +57,17 @@ string Method::wrapper_fragment(FileWriter& wrapperFile,
   // example: shared_ptr<Test> = unwrap_shared_ptr< Test >(in[0], "Test");
   wrapperFile.oss << "  Shared obj = unwrap_shared_ptr<" << cppClassName
       << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
-  // unwrap arguments, see Argument.cpp
+
+  // unwrap arguments, see Argument.cpp, we start at 1 as first is obj
   args.matlab_unwrap(wrapperFile, 1);
 
   // call method and wrap result
-  // example: out[0]=wrap<bool>(self->return_field(t));
+  // example: out[0]=wrap<bool>(obj->return_field(t));
   string expanded = "obj->" + name_;
   if (!instName.empty())
     expanded += ("<" + instName.qualifiedName("::") + ">");
-  expanded += ("(" + args.names() + ")");
-  if (returnVal.type1.name != "void")
-    returnVal.wrap_result(expanded, wrapperFile, typeAttributes);
-  else
-    wrapperFile.oss << "  " + expanded + ";\n";
 
-  // finish
-  wrapperFile.oss << "}\n";
-
-  return wrapFunctionName;
+  return expanded;
 }
 
 /* ************************************************************************* */
diff --git a/wrap/Method.h b/wrap/Method.h
index 36a53b3d7..8b8c7eaab 100644
--- a/wrap/Method.h
+++ b/wrap/Method.h
@@ -18,16 +18,16 @@
 
 #pragma once
 
-#include "Function.h"
+#include "StaticMethod.h"
 
 namespace wrap {
 
 /// Method class
-struct Method : public Function {
+struct Method: public StaticMethod {
 
   /// Constructor creates empty object
   Method(bool verbose = true) :
-      Function(verbose), is_const_(false) {
+      StaticMethod(verbose), is_const_(false) {
   }
 
   bool is_const_;
@@ -39,21 +39,16 @@ struct Method : public Function {
       const ArgumentList& args, const ReturnValue& retVal,
       const Qualified& instName = Qualified());
 
-  // MATLAB code generation
-  // classPath is class directory, e.g., ../matlab/@Point2
-  void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
-      const std::string& cppClassName, const std::string& matlabQualName,
-      const std::string& matlabUniqueName, const std::string& wrapperName,
-      const TypeAttributesTable& typeAttributes,
-      std::vector<std::string>& functionNames) const;
-
 private:
 
-  /// Emit C++ code
-  std::string wrapper_fragment(FileWriter& wrapperFile,
+  // Emit method header
+  void proxy_header(FileWriter& proxyFile) const;
+
+  std::string wrapper_call(FileWriter& wrapperFile,
       const std::string& cppClassName, const std::string& matlabUniqueName,
-      int overload, int id, const TypeAttributesTable& typeAttributes,
-      const Qualified& instName) const; ///< cpp wrapper
+      const ArgumentList& args, const ReturnValue& returnVal,
+      const TypeAttributesTable& typeAttributes,
+      const Qualified& instName) const;
 };
 
 } // \namespace wrap
diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp
index 84d662e81..a511652e8 100644
--- a/wrap/ReturnValue.cpp
+++ b/wrap/ReturnValue.cpp
@@ -59,18 +59,17 @@ void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const {
 }
 
 /* ************************************************************************* */
-ReturnValue ReturnValue::expandTemplate(const string& templateArg,
-    const Qualified& qualifiedType, const Qualified& expandedClass) const {
+ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const {
   ReturnValue instRetVal = *this;
-  if (type1.name == templateArg) {
-    instRetVal.type1.rename(qualifiedType);
+  if (type1.name == ts.templateArg) {
+    instRetVal.type1.rename(ts.qualifiedType);
   } else if (type1.name == "This") {
-    instRetVal.type1.rename(expandedClass);
+    instRetVal.type1.rename(ts.expandedClass);
   }
-  if (type2.name == templateArg) {
-    instRetVal.type2.rename(qualifiedType);
+  if (type2.name == ts.templateArg) {
+    instRetVal.type2.rename(ts.qualifiedType);
   } else if (type2.name == "This") {
-    instRetVal.type2.rename(expandedClass);
+    instRetVal.type2.rename(ts.expandedClass);
   }
   return instRetVal;
 }
diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h
index 1caaeae22..55ebf8c57 100644
--- a/wrap/ReturnValue.h
+++ b/wrap/ReturnValue.h
@@ -8,7 +8,7 @@
  * @author Richard Roberts
  */
 
-#include "Qualified.h"
+#include "TemplateSubstitution.h"
 #include "FileWriter.h"
 #include "TypeAttributesTable.h"
 #include "utilities.h"
@@ -86,21 +86,7 @@ struct ReturnValue {
   }
 
   /// Substitute template argument
-  ReturnValue expandTemplate(const std::string& templateArg,
-      const Qualified& qualifiedType, const Qualified& expandedClass) const;
-
-  // TODO use transform ?
-  static std::vector<ReturnValue> ExpandTemplate(
-      std::vector<ReturnValue> returnVals, const std::string& templateArg,
-      const Qualified& qualifiedType, const Qualified& expandedClass) {
-    std::vector<ReturnValue> result;
-    BOOST_FOREACH(const ReturnValue& retVal, returnVals) {
-      ReturnValue instRetVal = retVal.expandTemplate(templateArg,
-          qualifiedType, expandedClass);
-      result.push_back(instRetVal);
-    }
-    return result;
-  }
+  ReturnValue expandTemplate(const TemplateSubstitution& ts) const;
 
   std::string return_type(bool add_ptr) const;
 
diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp
index f8eba744f..a4d54f4dd 100644
--- a/wrap/StaticMethod.cpp
+++ b/wrap/StaticMethod.cpp
@@ -16,7 +16,7 @@
  * @author Richard Roberts
  **/
 
-#include "StaticMethod.h"
+#include "Method.h"
 #include "utilities.h"
 
 #include <boost/foreach.hpp>
@@ -30,108 +30,148 @@ using namespace std;
 using namespace wrap;
 
 /* ************************************************************************* */
-void StaticMethod::proxy_wrapper_fragments(FileWriter& file,
+void StaticMethod::addOverload(bool verbose, const std::string& name,
+    const ArgumentList& args, const ReturnValue& retVal,
+    const Qualified& instName) {
+
+  Function::addOverload(verbose, name, instName);
+  SignatureOverloads::addOverload(args, retVal);
+}
+
+/* ************************************************************************* */
+void StaticMethod::proxy_header(FileWriter& proxyFile) const {
+  string upperName = name_;
+  upperName[0] = std::toupper(upperName[0], std::locale());
+  proxyFile.oss << "    function varargout = " << upperName << "(varargin)\n";
+}
+
+/* ************************************************************************* */
+void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile,
     FileWriter& wrapperFile, const string& cppClassName,
     const std::string& matlabQualName, const std::string& matlabUniqueName,
     const string& wrapperName, const TypeAttributesTable& typeAttributes,
     vector<string>& functionNames) const {
 
-  string upperName = name_;
-  upperName[0] = std::toupper(upperName[0], std::locale());
+  proxy_header(proxyFile);
 
-  file.oss << "    function varargout = " << upperName << "(varargin)\n";
-  //Comments for documentation
+  // Emit comments for documentation
   string up_name = boost::to_upper_copy(name_);
-  file.oss << "      % " << up_name << " usage: ";
-  unsigned int argLCount = 0;
-  BOOST_FOREACH(ArgumentList argList, argLists) {
-    argList.emit_prototype(file, name_);
-    if (argLCount != argLists.size() - 1)
-      file.oss << ", ";
-    else
-      file.oss << " : returns "
-          << returnVals[0].return_type(false) << endl;
-    argLCount++;
-  }
+  proxyFile.oss << "      % " << up_name << " usage: ";
+  usage_fragment(proxyFile, name_);
 
-  file.oss << "      % "
+  // Emit URL to Doxygen page
+  proxyFile.oss << "      % "
       << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html"
       << endl;
-  file.oss << "      % " << "" << endl;
-  file.oss << "      % " << "Usage" << endl;
-  BOOST_FOREACH(ArgumentList argList, argLists) {
-    file.oss << "      % ";
-    argList.emit_prototype(file, up_name);
-    file.oss << endl;
-  }
-
-  // Check arguments for all overloads
-  for (size_t overload = 0; overload < argLists.size(); ++overload) {
 
+  // Handle special case of single overload with all numeric arguments
+  if (nrOverloads() == 1 && argumentList(0).allScalar()) {
     // Output proxy matlab code
-    file.oss << "      " << (overload == 0 ? "" : "else");
+    proxyFile.oss << "      ";
     const int id = (int) functionNames.size();
-    argLists[overload].emit_conditional_call(file, returnVals[overload],
-        wrapperName, id, true); // last bool is to indicate static !
+    argumentList(0).emit_call(proxyFile, returnValue(0), wrapperName, id);
 
     // Output C++ wrapper code
     const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName,
-        matlabUniqueName, (int) overload, id, typeAttributes);
+        matlabUniqueName, 0, id, typeAttributes, templateArgValue_);
 
     // Add to function list
     functionNames.push_back(wrapFunctionName);
-  }
-  file.oss << "      else\n";
-  file.oss << "        error('Arguments do not match any overload of function "
-      << matlabQualName << "." << upperName << "');" << endl;
-  file.oss << "      end\n";
+  } else {
+    // Check arguments for all overloads
+    for (size_t i = 0; i < nrOverloads(); ++i) {
 
-  file.oss << "    end\n";
+      // Output proxy matlab code
+      proxyFile.oss << "      " << (i == 0 ? "" : "else");
+      const int id = (int) functionNames.size();
+      string expanded = wrapperName;
+      if (!templateArgValue_.empty())
+        expanded += templateArgValue_.name;
+      argumentList(i).emit_conditional_call(proxyFile, returnValue(i), expanded,
+          id);
+
+      // Output C++ wrapper code
+      const string wrapFunctionName = wrapper_fragment(wrapperFile,
+          cppClassName, matlabUniqueName, i, id, typeAttributes,
+          templateArgValue_);
+
+      // Add to function list
+      functionNames.push_back(wrapFunctionName);
+    }
+    proxyFile.oss << "      else\n";
+    proxyFile.oss
+        << "        error('Arguments do not match any overload of function "
+        << matlabQualName << "." << name_ << "');" << endl;
+    proxyFile.oss << "      end\n";
+  }
+
+  proxyFile.oss << "    end\n";
 }
 
 /* ************************************************************************* */
-string StaticMethod::wrapper_fragment(FileWriter& file,
+string StaticMethod::wrapper_fragment(FileWriter& wrapperFile,
     const string& cppClassName, const string& matlabUniqueName, int overload,
-    int id, const TypeAttributesTable& typeAttributes) const {
+    int id, const TypeAttributesTable& typeAttributes,
+    const Qualified& instName) const {
 
   // generate code
 
   const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_"
       + boost::lexical_cast<string>(id);
 
-  const ArgumentList& args = argLists[overload];
-  const ReturnValue& returnVal = returnVals[overload];
+  const ArgumentList& args = argumentList(overload);
+  const ReturnValue& returnVal = returnValue(overload);
 
   // call
-  file.oss << "void " << wrapFunctionName
+  wrapperFile.oss << "void " << wrapFunctionName
       << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
   // start
-  file.oss << "{\n";
+  wrapperFile.oss << "{\n";
 
-  returnVal.wrapTypeUnwrap(file);
+  returnVal.wrapTypeUnwrap(wrapperFile);
 
-  file.oss << "  typedef boost::shared_ptr<" << cppClassName << "> Shared;"
-      << endl;
+  wrapperFile.oss << "  typedef boost::shared_ptr<" << cppClassName
+      << "> Shared;" << endl;
 
-  // check arguments
-  // NOTE: for static functions, there is no object passed
-  file.oss << "  checkArguments(\"" << matlabUniqueName << "." << name_
-      << "\",nargout,nargin," << args.size() << ");\n";
+  // get call
+  // for static methods: cppClassName::staticMethod<TemplateVal>
+  // for instance methods: obj->instanceMethod<TemplateVal>
+  string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName,
+      args, returnVal, typeAttributes, instName);
 
-  // unwrap arguments, see Argument.cpp
-  args.matlab_unwrap(file, 0); // We start at 0 because there is no self object
-
-  // call method with default type and wrap result
+  expanded += ("(" + args.names() + ")");
   if (returnVal.type1.name != "void")
-    returnVal.wrap_result(cppClassName + "::" + name_ + "(" + args.names() + ")",
-        file, typeAttributes);
+    returnVal.wrap_result(expanded, wrapperFile, typeAttributes);
   else
-    file.oss << cppClassName + "::" + name_ + "(" + args.names() + ");\n";
+    wrapperFile.oss << "  " + expanded + ";\n";
 
   // finish
-  file.oss << "}\n";
+  wrapperFile.oss << "}\n";
 
   return wrapFunctionName;
 }
 
 /* ************************************************************************* */
+string StaticMethod::wrapper_call(FileWriter& wrapperFile,
+    const string& cppClassName, const string& matlabUniqueName,
+    const ArgumentList& args, const ReturnValue& returnVal,
+    const TypeAttributesTable& typeAttributes,
+    const Qualified& instName) const {
+  // check arguments
+  // NOTE: for static functions, there is no object passed
+  wrapperFile.oss << "  checkArguments(\"" << matlabUniqueName << "." << name_
+      << "\",nargout,nargin," << args.size() << ");\n";
+
+  // unwrap arguments, see Argument.cpp
+  args.matlab_unwrap(wrapperFile, 0); // We start at 0 because there is no self object
+
+  // call method and wrap result
+  // example: out[0]=wrap<bool>(staticMethod(t));
+  string expanded = cppClassName + "::" + name_;
+  if (!instName.empty())
+    expanded += ("<" + instName.qualifiedName("::") + ">");
+
+  return expanded;
+}
+
+/* ************************************************************************* */
diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h
index 14162b3c8..9b9740bdb 100644
--- a/wrap/StaticMethod.h
+++ b/wrap/StaticMethod.h
@@ -24,13 +24,17 @@
 namespace wrap {
 
 /// StaticMethod class
-struct StaticMethod: public Function {
+struct StaticMethod: public Function, public SignatureOverloads {
 
   /// Constructor creates empty object
   StaticMethod(bool verbosity = true) :
       Function(verbosity) {
   }
 
+  void addOverload(bool verbose, const std::string& name,
+      const ArgumentList& args, const ReturnValue& retVal,
+      const Qualified& instName);
+
   // MATLAB code generation
   // classPath is class directory, e.g., ../matlab/@Point2
   void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
@@ -39,10 +43,20 @@ struct StaticMethod: public Function {
       const TypeAttributesTable& typeAttributes,
       std::vector<std::string>& functionNames) const;
 
-private:
-  std::string wrapper_fragment(FileWriter& file,
+protected:
+
+  virtual void proxy_header(FileWriter& proxyFile) const;
+
+  std::string wrapper_fragment(FileWriter& wrapperFile,
       const std::string& cppClassName, const std::string& matlabUniqueName,
-      int overload, int id, const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper
+      int overload, int id, const TypeAttributesTable& typeAttributes,
+      const Qualified& instName = Qualified()) const; ///< cpp wrapper
+
+  virtual std::string wrapper_call(FileWriter& wrapperFile,
+      const std::string& cppClassName, const std::string& matlabUniqueName,
+      const ArgumentList& args, const ReturnValue& returnVal,
+      const TypeAttributesTable& typeAttributes,
+      const Qualified& instName) const;
 };
 
 } // \namespace wrap
diff --git a/wrap/TemplateInstantiationTypedef.cpp b/wrap/TemplateInstantiationTypedef.cpp
index b93a05a54..2007acdf1 100644
--- a/wrap/TemplateInstantiationTypedef.cpp
+++ b/wrap/TemplateInstantiationTypedef.cpp
@@ -46,8 +46,10 @@ Class TemplateInstantiationTypedef::findAndExpand(
 
   // Instantiate it
   Class classInst = *matchedClass;
-  for (size_t i = 0; i < typeList.size(); ++i)
-    classInst = classInst.expandTemplate(classInst.templateArgs[i], typeList[i], *this);
+  for (size_t i = 0; i < typeList.size(); ++i) {
+    TemplateSubstitution ts(classInst.templateArgs[i], typeList[i], *this);
+    classInst = classInst.expandTemplate(ts);
+  }
 
   // Fix class properties
   classInst.name = name;
diff --git a/wrap/TemplateSubstitution.h b/wrap/TemplateSubstitution.h
new file mode 100644
index 000000000..5ce2f3a86
--- /dev/null
+++ b/wrap/TemplateSubstitution.h
@@ -0,0 +1,39 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file TemplateSubstitution.h
+ * @brief Auxiliary class for template sunstitutions
+ * @author Frank Dellaert
+ * @date Nov 13, 2014
+ **/
+
+#pragma once
+
+#include "Qualified.h"
+#include <string>
+
+namespace wrap {
+
+/**
+ * e.g. TemplateSubstitution("T", gtsam::Point2, gtsam::PriorFactorPoint2)
+ */
+struct TemplateSubstitution {
+  TemplateSubstitution(const std::string& a, const Qualified& t,
+      const Qualified& e) :
+      templateArg(a), qualifiedType(t), expandedClass(e) {
+  }
+  std::string templateArg;
+  Qualified qualifiedType, expandedClass;
+};
+
+} // \namespace wrap
+
diff --git a/wrap/tests/expected2/+gtsam/Point2.m b/wrap/tests/expected2/+gtsam/Point2.m
new file mode 100644
index 000000000..308b35d9a
--- /dev/null
+++ b/wrap/tests/expected2/+gtsam/Point2.m
@@ -0,0 +1,90 @@
+%class Point2, see Doxygen page for details
+%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%
+%-------Constructors-------
+%Point2()
+%Point2(double x, double y)
+%
+%-------Methods-------
+%argChar(char a) : returns void
+%argUChar(unsigned char a) : returns void
+%dim() : returns int
+%returnChar() : returns char
+%vectorConfusion() : returns VectorNotEigen
+%x() : returns double
+%y() : returns double
+%
+classdef Point2 < handle
+  properties
+    ptr_gtsamPoint2 = 0
+  end
+  methods
+    function obj = Point2(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        geometry_wrapper(0, my_ptr);
+      elseif nargin == 0
+        my_ptr = geometry_wrapper(1);
+      elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
+        my_ptr = geometry_wrapper(2, varargin{1}, varargin{2});
+      else
+        error('Arguments do not match any overload of gtsam.Point2 constructor');
+      end
+      obj.ptr_gtsamPoint2 = my_ptr;
+    end
+
+    function delete(obj)
+      geometry_wrapper(3, obj.ptr_gtsamPoint2);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+    function varargout = argChar(this, varargin)
+      % ARGCHAR usage: argChar(char a) : returns void
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      geometry_wrapper(4, this, varargin{:});
+    end
+
+    function varargout = argUChar(this, varargin)
+      % ARGUCHAR usage: argUChar(unsigned char a) : returns void
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      geometry_wrapper(5, this, varargin{:});
+    end
+
+    function varargout = dim(this, varargin)
+      % DIM usage: dim() : returns int
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      varargout{1} = geometry_wrapper(6, this, varargin{:});
+    end
+
+    function varargout = returnChar(this, varargin)
+      % RETURNCHAR usage: returnChar() : returns char
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      varargout{1} = geometry_wrapper(7, this, varargin{:});
+    end
+
+    function varargout = vectorConfusion(this, varargin)
+      % VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      varargout{1} = geometry_wrapper(8, this, varargin{:});
+    end
+
+    function varargout = x(this, varargin)
+      % X usage: x() : returns double
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      varargout{1} = geometry_wrapper(9, this, varargin{:});
+    end
+
+    function varargout = y(this, varargin)
+      % Y usage: y() : returns double
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      varargout{1} = geometry_wrapper(10, this, varargin{:});
+    end
+
+  end
+
+  methods(Static = true)
+  end
+end
diff --git a/wrap/tests/expected2/+gtsam/Point3.m b/wrap/tests/expected2/+gtsam/Point3.m
new file mode 100644
index 000000000..d445c78ef
--- /dev/null
+++ b/wrap/tests/expected2/+gtsam/Point3.m
@@ -0,0 +1,75 @@
+%class Point3, see Doxygen page for details
+%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%
+%-------Constructors-------
+%Point3(double x, double y, double z)
+%
+%-------Methods-------
+%norm() : returns double
+%
+%-------Static Methods-------
+%StaticFunctionRet(double z) : returns gtsam::Point3
+%staticFunction() : returns double
+%
+classdef Point3 < handle
+  properties
+    ptr_gtsamPoint3 = 0
+  end
+  methods
+    function obj = Point3(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        geometry_wrapper(11, my_ptr);
+      elseif nargin == 3 && isa(varargin{1},'double') && isa(varargin{2},'double') && isa(varargin{3},'double')
+        my_ptr = geometry_wrapper(12, varargin{1}, varargin{2}, varargin{3});
+      else
+        error('Arguments do not match any overload of gtsam.Point3 constructor');
+      end
+      obj.ptr_gtsamPoint3 = my_ptr;
+    end
+
+    function delete(obj)
+      geometry_wrapper(13, obj.ptr_gtsamPoint3);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+    function varargout = norm(this, varargin)
+      % NORM usage: norm() : returns double
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      varargout{1} = geometry_wrapper(14, this, varargin{:});
+    end
+
+  end
+
+  methods(Static = true)
+    function varargout = StaticFunctionRet(varargin)
+      % STATICFUNCTIONRET usage: StaticFunctionRet(double z) : returns gtsam::Point3
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % 
+      % Usage
+      % STATICFUNCTIONRET(double z)
+      if length(varargin) == 1 && isa(varargin{1},'double')
+        varargout{1} = geometry_wrapper(15, varargin{:});
+      else
+        error('Arguments do not match any overload of function gtsam.Point3.StaticFunctionRet');
+      end
+    end
+
+    function varargout = StaticFunction(varargin)
+      % STATICFUNCTION usage: staticFunction() : returns double
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % 
+      % Usage
+      % STATICFUNCTION()
+      if length(varargin) == 0
+        varargout{1} = geometry_wrapper(16, varargin{:});
+      else
+        error('Arguments do not match any overload of function gtsam.Point3.StaticFunction');
+      end
+    end
+
+  end
+end
diff --git a/wrap/tests/expected_namespaces/+ns2/ClassA.m b/wrap/tests/expected_namespaces/+ns2/ClassA.m
index 9c064734e..9f0055af9 100644
--- a/wrap/tests/expected_namespaces/+ns2/ClassA.m
+++ b/wrap/tests/expected_namespaces/+ns2/ClassA.m
@@ -65,14 +65,7 @@ classdef ClassA < handle
     function varargout = Afunction(varargin)
       % AFUNCTION usage: afunction() : returns double
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      % 
-      % Usage
-      % AFUNCTION()
-      if length(varargin) == 0
-        varargout{1} = testNamespaces_wrapper(12, varargin{:});
-      else
-        error('Arguments do not match any overload of function ns2.ClassA.Afunction');
-      end
+      varargout{1} = testNamespaces_wrapper(12, this, varargin{:});
     end
 
   end
diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp
index 743370c6d..02e618668 100644
--- a/wrap/tests/testWrap.cpp
+++ b/wrap/tests/testWrap.cpp
@@ -99,10 +99,9 @@ TEST( wrap, Small ) {
   Method m1 = cls.method("x");
   EXPECT(assert_equal("x", m1.name_));
   EXPECT(m1.is_const_);
-  LONGS_EQUAL(1, m1.argLists.size());
-  LONGS_EQUAL(1, m1.returnVals.size());
+  LONGS_EQUAL(1, m1.nrOverloads());
 
-  ReturnValue rv1 = m1.returnVals.front();
+  ReturnValue rv1 = m1.returnValue(0);
   EXPECT(!rv1.isPair);
   EXPECT(!rv1.type1.isPtr);
   EXPECT(assert_equal("double", rv1.type1.name));
@@ -112,10 +111,9 @@ TEST( wrap, Small ) {
   Method m2 = cls.method("returnMatrix");
   EXPECT(assert_equal("returnMatrix", m2.name_));
   EXPECT(m2.is_const_);
-  LONGS_EQUAL(1, m2.argLists.size());
-  LONGS_EQUAL(1, m2.returnVals.size());
+  LONGS_EQUAL(1, m2.nrOverloads());
 
-  ReturnValue rv2 = m2.returnVals.front();
+  ReturnValue rv2 = m2.returnValue(0);
   EXPECT(!rv2.isPair);
   EXPECT(!rv2.type1.isPtr);
   EXPECT(assert_equal("Matrix", rv2.type1.name));
@@ -125,10 +123,9 @@ TEST( wrap, Small ) {
   Method m3 = cls.method("returnPoint2");
   EXPECT(assert_equal("returnPoint2", m3.name_));
   EXPECT(m3.is_const_);
-  LONGS_EQUAL(1, m3.argLists.size());
-  LONGS_EQUAL(1, m3.returnVals.size());
+  LONGS_EQUAL(1, m3.nrOverloads());
 
-  ReturnValue rv3 = m3.returnVals.front();
+  ReturnValue rv3 = m3.returnValue(0);
   EXPECT(!rv3.isPair);
   EXPECT(!rv3.type1.isPtr);
   EXPECT(assert_equal("Point2", rv3.type1.name));
@@ -138,10 +135,9 @@ TEST( wrap, Small ) {
   // static Vector returnVector();
   StaticMethod sm1 = cls.static_methods.at("returnVector");
   EXPECT(assert_equal("returnVector", sm1.name_));
-  LONGS_EQUAL(1, sm1.argLists.size());
-  LONGS_EQUAL(1, sm1.returnVals.size());
+  LONGS_EQUAL(1, sm1.nrOverloads());
 
-  ReturnValue rv4 = sm1.returnVals.front();
+  ReturnValue rv4 = sm1.returnValue(0);
   EXPECT(!rv4.isPair);
   EXPECT(!rv4.type1.isPtr);
   EXPECT(assert_equal("Vector", rv4.type1.name));
@@ -195,13 +191,13 @@ TEST( wrap, Geometry ) {
       //   char returnChar() const;
       CHECK(cls.exists("returnChar"));
       Method m1 = cls.method("returnChar");
-      LONGS_EQUAL(1, m1.returnVals.size());
-      EXPECT(assert_equal("char", m1.returnVals.front().type1.name));
-      EXPECT_LONGS_EQUAL(ReturnType::BASIS, m1.returnVals.front().type1.category);
-      EXPECT(!m1.returnVals.front().isPair);
+      LONGS_EQUAL(1, m1.nrOverloads());
+      EXPECT(assert_equal("char", m1.returnValue(0).type1.name));
+      EXPECT_LONGS_EQUAL(ReturnType::BASIS, m1.returnValue(0).type1.category);
+      EXPECT(!m1.returnValue(0).isPair);
       EXPECT(assert_equal("returnChar", m1.name_));
-      LONGS_EQUAL(1, m1.argLists.size());
-      EXPECT_LONGS_EQUAL(0, m1.argLists.front().size());
+      LONGS_EQUAL(1, m1.nrOverloads());
+      EXPECT_LONGS_EQUAL(0, m1.argumentList(0).size());
       EXPECT(m1.is_const_);
     }
 
@@ -209,13 +205,13 @@ TEST( wrap, Geometry ) {
       //   VectorNotEigen vectorConfusion();
       CHECK(cls.exists("vectorConfusion"));
       Method m1 = cls.method("vectorConfusion");
-      LONGS_EQUAL(1, m1.returnVals.size());
-      EXPECT(assert_equal("VectorNotEigen", m1.returnVals.front().type1.name));
-      EXPECT_LONGS_EQUAL(ReturnType::CLASS, m1.returnVals.front().type1.category);
-      EXPECT(!m1.returnVals.front().isPair);
+      LONGS_EQUAL(1, m1.nrOverloads());
+      EXPECT(assert_equal("VectorNotEigen", m1.returnValue(0).type1.name));
+      EXPECT_LONGS_EQUAL(ReturnType::CLASS, m1.returnValue(0).type1.category);
+      EXPECT(!m1.returnValue(0).isPair);
       EXPECT(assert_equal("vectorConfusion", m1.name_));
-      LONGS_EQUAL(1, m1.argLists.size());
-      EXPECT_LONGS_EQUAL(0, m1.argLists.front().size());
+      LONGS_EQUAL(1, m1.nrOverloads());
+      EXPECT_LONGS_EQUAL(0, m1.argumentList(0).size());
       EXPECT(!m1.is_const_);
     }
 
@@ -252,12 +248,12 @@ TEST( wrap, Geometry ) {
     // check method
     CHECK(cls.exists("norm"));
     Method m1 = cls.method("norm");
-    LONGS_EQUAL(1, m1.returnVals.size());
-    EXPECT(assert_equal("double", m1.returnVals.front().type1.name));
-    EXPECT_LONGS_EQUAL(ReturnType::BASIS, m1.returnVals.front().type1.category);
+    LONGS_EQUAL(1, m1.nrOverloads());
+    EXPECT(assert_equal("double", m1.returnValue(0).type1.name));
+    EXPECT_LONGS_EQUAL(ReturnType::BASIS, m1.returnValue(0).type1.category);
     EXPECT(assert_equal("norm", m1.name_));
-    LONGS_EQUAL(1, m1.argLists.size());
-    EXPECT_LONGS_EQUAL(0, m1.argLists.front().size());
+    LONGS_EQUAL(1, m1.nrOverloads());
+    EXPECT_LONGS_EQUAL(0, m1.argumentList(0).size());
     EXPECT(m1.is_const_);
 
 #ifndef WRAP_DISABLE_SERIALIZE
@@ -278,19 +274,19 @@ TEST( wrap, Geometry ) {
     // function to parse: pair<Vector,Matrix> return_pair (Vector v, Matrix A) const;
     CHECK(testCls.exists("return_pair"));
     Method m2 = testCls.method("return_pair");
-    LONGS_EQUAL(1, m2.returnVals.size());
-    EXPECT(m2.returnVals.front().isPair);
-    EXPECT_LONGS_EQUAL(ReturnType::EIGEN, m2.returnVals.front().type1.category);
-    EXPECT(assert_equal("Vector", m2.returnVals.front().type1.name));
-    EXPECT_LONGS_EQUAL(ReturnType::EIGEN, m2.returnVals.front().type2.category);
-    EXPECT(assert_equal("Matrix", m2.returnVals.front().type2.name));
+    LONGS_EQUAL(1, m2.nrOverloads());
+    EXPECT(m2.returnValue(0).isPair);
+    EXPECT_LONGS_EQUAL(ReturnType::EIGEN, m2.returnValue(0).type1.category);
+    EXPECT(assert_equal("Vector", m2.returnValue(0).type1.name));
+    EXPECT_LONGS_EQUAL(ReturnType::EIGEN, m2.returnValue(0).type2.category);
+    EXPECT(assert_equal("Matrix", m2.returnValue(0).type2.name));
 
     // checking pointer args and return values
 //    pair<Test*,Test*> return_ptrs (Test* p1, Test* p2) const;
     CHECK(testCls.exists("return_ptrs"));
     Method m3 = testCls.method("return_ptrs");
-    LONGS_EQUAL(1, m3.argLists.size());
-    ArgumentList args = m3.argLists.front();
+    LONGS_EQUAL(1, m3.nrOverloads());
+    ArgumentList args = m3.argumentList(0);
     LONGS_EQUAL(2, args.size());
 
     Argument arg1 = args.at(0);
@@ -317,9 +313,9 @@ TEST( wrap, Geometry ) {
   {
     GlobalFunction gfunc = module.global_functions.at("aGlobalFunction");
     EXPECT(assert_equal("aGlobalFunction", gfunc.name_));
-    LONGS_EQUAL(1, gfunc.returnVals.size());
-    EXPECT(assert_equal("Vector", gfunc.returnVals.front().type1.name));
-    EXPECT_LONGS_EQUAL(1, gfunc.argLists.size());
+    LONGS_EQUAL(1, gfunc.nrOverloads());
+    EXPECT(assert_equal("Vector", gfunc.returnValue(0).type1.name));
+    EXPECT_LONGS_EQUAL(1, gfunc.nrOverloads());
     LONGS_EQUAL(1, gfunc.overloads.size());
     EXPECT(gfunc.overloads.front().namespaces.empty());
   }
@@ -391,9 +387,8 @@ TEST( wrap, parse_namespaces ) {
   {
     GlobalFunction gfunc = module.global_functions.at("aGlobalFunction");
     EXPECT(assert_equal("aGlobalFunction", gfunc.name_));
-    LONGS_EQUAL(2, gfunc.returnVals.size());
-    EXPECT(assert_equal("Vector", gfunc.returnVals.front().type1.name));
-    EXPECT_LONGS_EQUAL(2, gfunc.argLists.size());
+    LONGS_EQUAL(2, gfunc.nrOverloads());
+    EXPECT(assert_equal("Vector", gfunc.returnValue(0).type1.name));
 
     // check namespaces
     LONGS_EQUAL(2, gfunc.overloads.size());