#ifndef OPENCV_MXARRAY_HPP_
#define OPENCV_MXARRAY_HPP_

#include <stdint.h>
#include <cstdarg>
#include <string>
#include <vector>
#include <algorithm>
#include <sstream>
#include <opencv2/core.hpp>
#if __cplusplus > 201103
#include <unordered_set>
typedef std::unordered_set<std::string> StringSet;
#else
#include <set>
typedef std::set<std::string> StringSet;
#endif
#include "mex.h"
#include "transpose.hpp"

/*
 * All recent versions of Matlab ship with the MKL library which contains
 * a blas extension called mkl_?omatcopy(). This  defines an out-of-place 
 * copy and transpose operation.
 *
 * The mkl library is in ${MATLAB_ROOT}/bin/${MATLAB_MEXEXT}/libmkl...
 * Matlab does not ship headers for the mkl functions, so we define them
 * here.
 *
 * This operation is used extensively to copy between Matlab's column-major
 * format and OpenCV's row-major format.
 */
#ifdef __cplusplus
extern "C" {
#endif
#ifdef __cplusplus
}
#endif


/*!
 * @brief raise error if condition fails
 *
 * This is a conditional wrapper for mexErrMsgTxt. If the conditional
 * expression fails, an error is raised and the mex function returns
 * to Matlab, otherwise this function does nothing
 */
static void conditionalError(bool expr, const std::string& str) {
  if (!expr) mexErrMsgTxt(std::string("condition failed: ").append(str).c_str());
}

/*!
 * @brief raise an error
 *
 * This function is a wrapper around mexErrMsgTxt
 */
static void error(const std::string& str) {
  mexErrMsgTxt(str.c_str());
}


// ----------------------------------------------------------------------------
//                          PREDECLARATIONS
// ----------------------------------------------------------------------------
class MxArray;

template <typename InputScalar, typename OutputScalar>
void deepCopyAndTranspose(const cv::Mat& src, MxArray& dst);

template <typename InputScalar, typename OutputScalar>
void deepCopyAndTranspose(const MxArray& src, cv::Mat& dst);




// ----------------------------------------------------------------------------
//                            MATLAB TRAITS
// ----------------------------------------------------------------------------
namespace Matlab {
  class DefaultTraits {};
  class InheritType {};
  static const int Dynamic = -1;

  template<typename _Tp = DefaultTraits> class Traits {
  public:
    static const mxClassID ScalarType = mxUNKNOWN_CLASS;
    static const mxComplexity Complex = mxCOMPLEX;
    static const mxComplexity Real    = mxCOMPLEX;
    static std::string ToString()  { return "Unknown/Unsupported"; }
  };
  // bool
  template<> class Traits<bool> {
  public:
    static const mxClassID ScalarType = mxLOGICAL_CLASS;
    static std::string ToString()  { return "boolean"; }
  };
  // uint8_t
  template<> class Traits<uint8_t> {
  public:
    static const mxClassID ScalarType = mxUINT8_CLASS;
    static std::string ToString()  { return "uint8_t"; }
  };
  // int8_t
  template<> class Traits<int8_t> {
  public:
    static const mxClassID ScalarType = mxINT8_CLASS;
    static std::string ToString()  { return "int8_t"; }
  };
  // uint16_t
  template<> class Traits<uint16_t> {
  public:
    static const mxClassID ScalarType = mxUINT16_CLASS;
    static std::string ToString()  { return "uint16_t"; }
  };
  // int16_t
  template<> class Traits<int16_t> {
  public:
    static const mxClassID ScalarType = mxINT16_CLASS;
    static std::string ToString()  { return "int16_t"; }
  };
  // uint32_t
  template<> class Traits<uint32_t> {
  public:
    static const mxClassID ScalarType = mxUINT32_CLASS;
    static std::string ToString()  { return "uint32_t"; }
  };
  // int32_t
  template<> class Traits<int32_t> {
  public:
    static const mxClassID ScalarType = mxINT32_CLASS;
    static std::string ToString()  { return "int32_t"; }
  };
  // uint64_t
  template<> class Traits<uint64_t> {
  public:
    static const mxClassID ScalarType = mxUINT64_CLASS;
    static std::string ToString()  { return "uint64_t"; }
  };
  // int64_t
  template<> class Traits<int64_t> {
  public:
    static const mxClassID ScalarType = mxINT64_CLASS;
    static std::string ToString()  { return "int64_t"; }
  };
  // float
  template<> class Traits<float> {
  public:
    static const mxClassID ScalarType = mxSINGLE_CLASS;
    static std::string ToString()  { return "float"; }
  };
  // double
  template<> class Traits<double> {
  public:
    static const mxClassID ScalarType = mxDOUBLE_CLASS;
    static std::string ToString()  { return "double"; }
  };
  // char
  template<> class Traits<char> {
  public:
    static const mxClassID ScalarType = mxCHAR_CLASS;
    static std::string ToString()  { return "char"; }
  };
  // inherited type
  template<> class Traits<Matlab::InheritType> {
  public:
    static std::string ToString()  { return "Inherited type"; }
  };
}



// ----------------------------------------------------------------------------
//                                MXARRAY
// ----------------------------------------------------------------------------


/*!
 * @class MxArray
 * @brief A thin wrapper around Matlab's mxArray types
 *
 * MxArray provides a thin object oriented wrapper around Matlab's
 * native mxArray type which exposes most of the functionality of the
 * Matlab interface, but in a more C++ manner. MxArray objects are scoped,
 * so you can freely create and destroy them without worrying about memory
 * management. If you wish to pass the underlying mxArray* representation
 * back to Matlab as an lvalue, see the releaseOwnership() method
 *
 * MxArrays can be directly converted into OpenCV mat objects and std::string
 * objects, since there is a natural mapping between these types. More
 * complex types are mapped through the Bridge which does custom conversions
 * such as MxArray --> cv::Keypoints, etc
 */
class MxArray {
private:
  mxArray* ptr_;
  bool owns_;

  /*!
   * @brief swap all members of this and other
   *
   * the swap method is used by the assignment and move constructors 
   * to swap the members of two MxArrays, leaving both in destructible states
   */
  friend void swap(MxArray& first, MxArray& second) {
    using std::swap;
    swap(first.ptr_,  second.ptr_);
    swap(first.owns_, second.owns_);
  }

  void dealloc() { 
    if (owns_ && ptr_) { mxDestroyArray(ptr_); ptr_ = NULL; owns_ = false; }
  }
public:
  // --------------------------------------------------------------------------
  //                              CONSTRUCTORS
  // --------------------------------------------------------------------------
  /*!
   * @brief default constructor
   *
   * Construct a valid 0x0 matrix (so all other methods do not need validity checks
   */
  MxArray() : ptr_(mxCreateDoubleMatrix(1, 1, Matlab::Traits<>::Real)), owns_(true) {}

  /*!
   * @brief inheriting constructor
   *
   * Inherit an mxArray from Matlab. Don't claim ownership of the array,
   * just encapsulate it
   */
  MxArray(const mxArray* ptr) : ptr_(const_cast<mxArray *>(ptr)), owns_(false) {}
  MxArray& operator=(const mxArray* ptr) { 
    dealloc();
    ptr_ = const_cast<mxArray *>(ptr);
    owns_ = false;
    return *this;
  }

  /*!
   * @brief explicit typed constructor
   *
   * This constructor explicitly creates an MxArray of the given size and type.
   */
  MxArray(size_t m, size_t n, size_t k, mxClassID id, mxComplexity com = Matlab::Traits<>::Real) : owns_(true) {
    mwSize dims[] = { static_cast<mwSize>(m), static_cast<mwSize>(n), static_cast<mwSize>(k) };
    ptr_ = mxCreateNumericArray(3, dims, id, com);
  }

  /*!
   * @brief explicit tensor constructor
   *
   * Explicitly construct a tensor of given size and type. Since constructors cannot
   * be explicitly templated, this is a static factory method
   */
  template <typename Scalar>
  static MxArray Tensor(size_t m, size_t n, size_t k=1) {
    return MxArray(m, n, k, Matlab::Traits<Scalar>::ScalarType);
  }

  /*!
   * @brief explicit matrix constructor
   *
   * Explicitly construct a matrix of given size and type. Since constructors cannot
   * be explicitly templated, this is a static factory method
   */
  template <typename Scalar>
  static MxArray Matrix(size_t m, size_t n) {
    return MxArray(m, n, 1, Matlab::Traits<Scalar>::ScalarType);
  }

  /*!
   * @brief explicit vector constructor
   *
   * Explicitly construct a vector of given size and type. Since constructors cannot
   * be explicitly templated, this is a static factory method
   */
  template <typename Scalar>
  static MxArray Vector(size_t m) {
    return MxArray(m, 1, 1, Matlab::Traits<Scalar>::ScalarType);
  }

  /*!
   * @brief explicit scalar constructor
   *
   * Explicitly construct a scalar of given type. Since constructors cannot
   * be explicitly templated, this is a static factory method
   */
  template <typename ScalarType>
  static MxArray Scalar(ScalarType value = 0) {
    MxArray s(1, 1, 1, Matlab::Traits<ScalarType>::ScalarType);
    s.real<ScalarType>()[0] = value;
    return s;
  }

  /*! 
   * @brief destructor
   * 
   * The destructor deallocates any data allocated by mxCreate* methods only
   * if the object is owned
   */
  virtual ~MxArray() {
    dealloc();
  }

  /*! 
   * @brief copy constructor
   *
   * All copies are deep copies. If you have a C++11 compatible compiler, prefer
   * move construction to copy construction
   */
  MxArray(const MxArray& other) : ptr_(mxDuplicateArray(other.ptr_)), owns_(true) {}

  /*!
   * @brief copy-and-swap assignment
   *
   * This assignment operator uses the copy and swap idiom to provide a strong
   * exception guarantee when swapping two objects. 
   *
   * Note in particular that the other MxArray is passed by value, thus invoking
   * the copy constructor which performs a deep copy of the input. The members of
   * this and other are then swapped
   */
  MxArray& operator=(MxArray other) {
    swap(*this, other);
    return *this;
  }
#if __cplusplus >= 201103L
  /*
   * @brief C++11 move constructor
   *
   * When C++11 support is available, move construction is used to move returns
   * out of functions, etc. This is much fast than copy construction, since the
   * move constructed object replaced itself with a default constructed MxArray,
   * which is of size 0 x 0.
   */
  MxArray(MxArray&& other) : MxArray() {
    swap(*this, other);
  }
#endif

  /*
   * @brief release ownership to allow return into Matlab workspace
   *
   * MxArray is not directly convertible back to mxArray types through assignment
   * because the MxArray may have been allocated on the free store, making it impossible
   * to know whether the returned pointer will be released by someone else or not.
   * 
   * Since Matlab requires mxArrays be passed back into the workspace, the only way
   * to achieve that is through this function, which explicitly releases ownership 
   * of the object, assuming the Matlab interpreter receving the object will delete
   * it at a later time
   *
   * e.g.
   * {
   *    MxArray A<double>(5, 5);        // allocates memory
   *    MxArray B<double>(5, 5);        // ditto
   *    plhs[0] = A;                    // not allowed!!
   *    plhs[0] = A.releaseOwnership(); // makes explicit that ownership is being released
   * } // end of scope. B is released, A isn't
   *
   */
  mxArray* releaseOwnership() {
    owns_ = false;
    return ptr_;
  }


  template <typename Scalar>
  static MxArray FromMat(const cv::Mat& mat) {
    MxArray arr(mat.rows, mat.cols, mat.channels(), Matlab::Traits<Scalar>::ScalarType);
    switch (mat.depth()) {
      case CV_8U:  deepCopyAndTranspose<uint8_t,  Scalar>(mat, arr); break;
      case CV_8S:  deepCopyAndTranspose<int8_t,   Scalar>(mat, arr); break;
      case CV_16U: deepCopyAndTranspose<uint16_t, Scalar>(mat, arr); break;
      case CV_16S: deepCopyAndTranspose<int16_t,  Scalar>(mat, arr); break;
      case CV_32S: deepCopyAndTranspose<int32_t,  Scalar>(mat, arr); break;
      case CV_32F: deepCopyAndTranspose<float,    Scalar>(mat, arr); break;
      case CV_64F: deepCopyAndTranspose<double,   Scalar>(mat, arr); break;
      default: error("Attempted to convert from unknown class");
    }
    return arr;
  }

  template <typename Scalar>
  cv::Mat toMat() const { 
    cv::Mat mat(rows(), cols(), CV_MAKETYPE(cv::DataType<Scalar>::type, channels()));
    switch (ID()) {
      case mxINT8_CLASS:    deepCopyAndTranspose<int8_t,   Scalar>(*this, mat); break;
      case mxUINT8_CLASS:   deepCopyAndTranspose<uint8_t,  Scalar>(*this, mat); break;
      case mxINT16_CLASS:   deepCopyAndTranspose<int16_t,  Scalar>(*this, mat); break;
      case mxUINT16_CLASS:  deepCopyAndTranspose<uint16_t, Scalar>(*this, mat); break;
      case mxINT32_CLASS:   deepCopyAndTranspose<int32_t,  Scalar>(*this, mat); break;
      case mxUINT32_CLASS:  deepCopyAndTranspose<uint32_t, Scalar>(*this, mat); break;
      case mxINT64_CLASS:   deepCopyAndTranspose<int64_t,  Scalar>(*this, mat); break;
      case mxUINT64_CLASS:  deepCopyAndTranspose<uint64_t, Scalar>(*this, mat); break;
      case mxSINGLE_CLASS:  deepCopyAndTranspose<float,    Scalar>(*this, mat); break;
      case mxDOUBLE_CLASS:  deepCopyAndTranspose<double,   Scalar>(*this, mat); break;
      case mxCHAR_CLASS:    deepCopyAndTranspose<char,     Scalar>(*this, mat); break;
      case mxLOGICAL_CLASS: deepCopyAndTranspose<int8_t,   Scalar>(*this, mat); break;
      default: error("Attempted to convert from unknown class");
    }
    return mat;
  }

  MxArray field(const std::string& name) { return MxArray(mxGetField(ptr_, 0, name.c_str())); }

  template <typename Scalar>
  Scalar* real() { return static_cast<Scalar *>(mxGetData(ptr_)); }
  
  template <typename Scalar>
  Scalar* imag() { return static_cast<Scalar *>(mxGetImagData(ptr_)); }

  template <typename Scalar>
  const Scalar* real() const { return static_cast<const Scalar *>(mxGetData(ptr_)); }
  
  template <typename Scalar>
  const Scalar* imag() const { return static_cast<const Scalar *>(mxGetData(ptr_)); }

  template <typename Scalar>
  Scalar scalar() const { return static_cast<Scalar *>(mxGetData(ptr_))[0]; }

  std::string toString() const {
    conditionalError(isString(), "Attempted to convert non-string type to string");
    std::string str(size(), '\0');
    mxGetString(ptr_, const_cast<char *>(str.data()), str.size()+1);
    return str;
  }

  size_t size() const { return mxGetNumberOfElements(ptr_); }
  size_t rows() const { return mxGetDimensions(ptr_)[0]; }
  size_t cols() const { return mxGetDimensions(ptr_)[1]; }
  size_t channels() const { return (mxGetNumberOfDimensions(ptr_) > 2) ? mxGetDimensions(ptr_)[2] : 1; }
  bool isComplex() const { return mxIsComplex(ptr_); }
  bool isNumeric() const { return mxIsNumeric(ptr_); }
  bool isLogical() const { return mxIsLogical(ptr_); }
  bool isString() const { return mxIsChar(ptr_); }
  bool isCell() const { return mxIsCell(ptr_); }
  bool isStructure() const { return mxIsStruct(ptr_); }
  bool isClass(const std::string& name) const { return mxIsClass(ptr_, name.c_str()); }
  std::string className() const { return std::string(mxGetClassName(ptr_)); }
  mxClassID ID() const { return mxGetClassID(ptr_); }

};


/*! @class ArgumentParser
 *  @brief parses inputs to a method and resolves the argument names.
 *
 * The ArgumentParser resolves the inputs to a method. It checks that all
 * required arguments are specified and also allows named optional arguments.
 * For example, the C++ function:
 *    void randn(Mat& mat, Mat& mean=Mat(), Mat& std=Mat());
 * could be called in Matlab using any of the following signatures:
 * \code
 *    out = randn(in);
 *    out = randn(in, 0, 1);
 *    out = randn(in, 'mean', 0, 'std', 1);
 * \endcode
 *
 * ArgumentParser also enables function overloading by allowing users
 * to add variants to a method. For example, there may be two C++ sum() methods:
 * \code
 *    double sum(Mat& mat);     % sum elements of a matrix
 *    Mat sum(Mat& A, Mat& B);  % add two matrices
 * \endcode
 *
 * by adding two variants to ArgumentParser, the correct underlying sum 
 * method can be called. If the function call is ambiguous, the 
 * ArgumentParser will fail with an error message.
 *
 * The previous example could be parsed as:
 * \code
 *    // set up the Argument parser
 *    ArgumentParser arguments;
 *    arguments.addVariant("elementwise", 1);
 *    arguments.addVariant("matrix", 2);
 *
 *    // parse the arguments
 *    std::vector<MxArray> inputs;
 *    inputs = arguments.parse(std::vector<MxArray>(prhs, prhs+nrhs));
 *
 *    // if we get here, one unique variant is valid
 *    if (arguments.variantIs("elementwise")) {
 *      // call elementwise sum()
 *    }
 */
class ArgumentParser {
private:
  struct Variant;
  typedef std::string String;
  typedef std::vector<std::string> StringVector;
  typedef std::vector<MxArray> MxArrayVector;
  typedef std::vector<Variant> VariantVector;
  /* @class Variant
   * @brief Describes a variant of arguments to a method
   *
   * When addVariant() is called on an instance to ArgumentParser, this class
   * holds the the information that decribes that variant. The parse() method
   * of ArgumentParser then attempts to match a Variant, given a set of 
   * inputs for a method invocation.
   */
  class Variant {
  public:
    Variant(const String& _name, size_t _nreq, size_t _nopt, const StringVector& _keys) 
      : name(_name), nreq(_nreq), nopt(_nopt), keys(_keys), using_named(false) {}
    String name;
    size_t nreq;
    size_t nopt;
    StringVector keys;
    bool using_named;
    /*! @brief return true if the named-argument is in the Variant */
    bool count(const String& key) { return std::find(keys.begin(), keys.end(), key) != keys.end(); }
    /*! @brief remove a key by index from the Variant */
    void erase(const size_t idx) { keys.erase(keys.begin()+idx); }
    /*! @brief remove a key by name from the Variant */ 
    void erase(const String& key) { keys.erase(std::find(keys.begin(), keys.end(), key)); }
    /*! @brief convert a Variant to a string representation */
    String toString(const String& method_name=String("f")) const {
      std::ostringstream s;
      s << method_name << "(";
      for (size_t n = 0; n < nreq; ++n) { 
        s << "src" << n+1; if (n != nreq-1) s << ", "; 
      }
      if (nreq && nopt) s << ", ";
      for (size_t n = 0; n < keys.size(); ++n) { 
        s << "'" << keys[n] << "', " << keys[n]; 
        if (n != keys.size()-1) s << ", ";
      }
      s << ");";
      return s.str();
    }
  };
  MxArrayVector filled_;
  VariantVector variants_;
  String valid_;
  String method_name_;
public:
  ArgumentParser(const String& method_name) : method_name_(method_name) {}
  /*! @brief add a function call variant to the parser
   *
   * Adds a function-call signature to the parser. The function call *must* be 
   * unique either in its number of arguments, or in the named-syntax.
   * Currently this function does not check whether that invariant stands true.
   *
   * This function is variadic. If should be called as follows:
   *  addVariant(2, 2, 'opt_1_name', 'opt_2_name');
   */
  void addVariant(const String& name, size_t nreq, size_t nopt = 0, ...) {
    StringVector keys;
    va_list opt;
    va_start(opt, nopt);
    for (size_t n = 0; n < nopt; ++n) keys.push_back(va_arg(opt, const char*));
    addVariant(name, nreq, nopt, keys);
  }
  void addVariant(const String& name, size_t nreq, size_t nopt, StringVector keys) {
    variants_.push_back(Variant(name, nreq, nopt, keys));
  }
  /*! @brief check if the valid variant is the key name */
  bool variantIs(const String& name) {
    return name.compare(valid_) == 0;
  }
  /*! @brief parse a vector of input arguments
   * 
   * This method parses a vector of input arguments, attempting to match them 
   * to a Variant spec. For each input, the method attempts to cull any 
   * Variants which don't match the given inputs so far.
   *
   * Once all inputs have been parsed, if there is one unique spec remaining,
   * the output MxArray vector gets populated with the arguments, with named
   * arguments removed. Any optional arguments that have not been encountered
   * are set to an empty array.
   *
   * If multiple variants or no variants match the given call, an error 
   * message is emitted
   */
  MxArrayVector parse(const MxArrayVector& inputs) {
    // allocate the outputs
    MxArrayVector outputs;
    VariantVector candidates = variants_;

    // iterate over the inputs, attempting to match a variant
    for (MxArrayVector::const_iterator input = inputs.begin(); input != inputs.end(); ++input) {
      String name = input->isString() ? input->toString() : String();
      for (VariantVector::iterator candidate = candidates.begin(); candidate < candidates.end(); ++candidate) {
        // check if the input is a key
        bool key = candidate->count(name);

        /* 
         * FAILURE CASES
         * 1. too many inputs, or
         * 2. name is not a key and we're expecting a key
         * 3. name is a key, and
         *    we're still expecting required arguments, or
         *    we're expecting an argument for a previous key
         */
        if ((!candidate->nreq && !candidate->nopt) ||
           (!key && !candidate->nreq && candidate->keys.size() == candidate->nopt && candidate->using_named) ||
            (key && (candidate->nreq || candidate->keys.size()  < candidate->nopt))) {
          candidate = candidates.erase(candidate)--;
        }

        // VALID CASES
        // Still parsing required argments (input is not a key)
        else if (!key && candidate->nreq) {
          candidate->nreq--;
        }

        // Parsing optional arguments and a named argument is encountered
        else if (key && !candidate->nreq && candidate->nopt > 0 && candidate->keys.size() == candidate->nopt) {
          candidate->erase(name);
          candidate->using_named = true;
        }

        // Parsing input for a named argument
        else if (!key && candidate->keys.size() < candidate->nopt) {
          candidate->nopt--; 
        }

        // Parsing un-named optional arguments
        else if (!key && !candidate->nreq && candidate->nopt && candidate->keys.size() && !candidate->using_named) {
          candidate->erase(0);
          candidate->nopt--;
        }
      }
    }

    // if any candidates remain, check that they have been fully parsed
    for (VariantVector::iterator candidate = candidates.begin(); candidate < candidates.end(); ++candidate) {
      if (candidate->nreq || candidate->keys.size() < candidate->nopt) {
        candidate = candidates.erase(candidate)--;
      }
    }

    // if there is not a unique candidate, throw an error
    String variant_string;
    for (VariantVector::iterator variant = variants_.begin(); variant != variants_.end(); ++variant) {
      variant_string += "\n" + variant->toString(method_name_);
    }

    // if there is not a unique candidate, throw an error
    if (candidates.size()  > 1) {
      error(String("Call to method is ambiguous. Valid variants are:")
        .append(variant_string).append("\nUse named arguments to disambiguate call"));
    }
    if (candidates.size() == 0) {
      error(String("No matching method signatures for given arguments. Valid variants are:").append(variant_string));
    }

    return outputs;
  }
};


/*!
 * @brief template specialization for inheriting types
 *
 * This template specialization attempts to preserve the best mapping
 * between OpenCV and Matlab types. Matlab uses double types almost universally, so
 * all floating float types are converted to doubles.
 * Unfortunately OpenCV does not have a native logical type, so
 * that gets mapped to an unsigned 8-bit value
 */
template <>
MxArray MxArray::FromMat<Matlab::InheritType>(const cv::Mat& mat) {
  switch (mat.depth()) {
    case CV_8U:  return FromMat<uint8_t>(mat);
    case CV_8S:  return FromMat<int8_t>(mat);
    case CV_16U: return FromMat<uint16_t>(mat);
    case CV_16S: return FromMat<int16_t>(mat);
    case CV_32S: return FromMat<int32_t>(mat);
    case CV_32F: return FromMat<double>(mat); //NOTE: Matlab uses double as native type!
    case CV_64F: return FromMat<double>(mat);
    default: error("Attempted to convert from unknown class");
  }
  return MxArray();
}

/*!
 * @brief template specialization for inheriting types
 * 
 * This template specialization attempts to preserve the best mapping
 * between Matlab and OpenCV types. OpenCV has poor support for double precision
 * types, so all floating point types are cast to float. Logicals get cast
 * to unsignd 8-bit value.
 */
template <>
cv::Mat MxArray::toMat<Matlab::InheritType>() const {
  switch (ID()) {
    case mxINT8_CLASS:    return toMat<int8_t>();
    case mxUINT8_CLASS:   return toMat<uint8_t>();
    case mxINT16_CLASS:   return toMat<int16_t>();
    case mxUINT16_CLASS:  return toMat<uint16_t>();
    case mxINT32_CLASS:   return toMat<int32_t>();
    case mxUINT32_CLASS:  return toMat<int32_t>();
    case mxINT64_CLASS:   return toMat<int64_t>();
    case mxUINT64_CLASS:  return toMat<int64_t>();
    case mxSINGLE_CLASS:  return toMat<float>();
    case mxDOUBLE_CLASS:  return toMat<float>(); //NOTE: OpenCV uses float as native type!
    case mxCHAR_CLASS:    return toMat<int8_t>();
    case mxLOGICAL_CLASS: return toMat<int8_t>();
    default: error("Attempted to convert from unknown class");
  }
  return cv::Mat();
}



// ----------------------------------------------------------------------------
//                            MATRIX TRANSPOSE
// ----------------------------------------------------------------------------


template <typename InputScalar, typename OutputScalar>
void deepCopyAndTranspose(const cv::Mat& in, MxArray& out) {
  conditionalError(static_cast<size_t>(in.rows) == out.rows(), "Matrices must have the same number of rows");
  conditionalError(static_cast<size_t>(in.cols) == out.cols(), "Matrices must have the same number of cols");
  conditionalError(static_cast<size_t>(in.channels()) == out.channels(), "Matrices must have the same number of channels");
  std::vector<cv::Mat> channels;
  cv::split(in, channels);
  for (size_t c = 0; c < out.channels(); ++c) {
    cv::transpose(channels[c], channels[c]);
    cv::Mat outmat(out.cols(), out.rows(), cv::DataType<OutputScalar>::type, 
      static_cast<void *>(out.real<OutputScalar>() + out.cols()*out.rows()*c));
    channels[c].convertTo(outmat, cv::DataType<OutputScalar>::type);
  }

  //const InputScalar* inp = in.ptr<InputScalar>(0);
  //OutputScalar* outp = out.real<OutputScalar>();
  //gemt('R', out.rows(), out.cols(), inp, in.step1(), outp, out.rows());
}

template <typename InputScalar, typename OutputScalar>
void deepCopyAndTranspose(const MxArray& in, cv::Mat& out) {
  conditionalError(in.rows() == static_cast<size_t>(out.rows), "Matrices must have the same number of rows");
  conditionalError(in.cols() == static_cast<size_t>(out.cols), "Matrices must have the same number of cols");
  conditionalError(in.channels() == static_cast<size_t>(out.channels()), "Matrices must have the same number of channels");
  std::vector<cv::Mat> channels;
  for (size_t c = 0; c < in.channels(); ++c) {
    cv::Mat outmat;
    cv::Mat inmat(in.cols(), in.rows(), cv::DataType<InputScalar>::type,
      static_cast<void *>(const_cast<InputScalar *>(in.real<InputScalar>() + in.cols()*in.rows()*c)));
    inmat.convertTo(outmat, cv::DataType<OutputScalar>::type);
    cv::transpose(outmat, outmat);
    channels.push_back(outmat);
  }
  cv::merge(channels, out);

  //const InputScalar* inp = in.real<InputScalar>();
  //OutputScalar* outp = out.ptr<OutputScalar>(0);
  //gemt('C', in.rows(), in.cols(), inp, in.rows(), outp, out.step1()); 
}



#endif