Matrix.cpp

#include "MantidKernel/Matrix.h"

#include "MantidKernel/Exception.h"
#include "MantidKernel/make_unique.h"
#include "MantidKernel/MersenneTwister.h"
#include "MantidKernel/TimeSeriesProperty.h"
#include "MantidKernel/V3D.h"

#include <algorithm>
#include <memory>
#include <sstream>

using Mantid::Kernel::TimeSeriesProperty;

namespace Mantid {

namespace Kernel {
//
#define fabs(x) std::fabs((x)*1.0)

namespace {
//-------------------------------------------------------------------------
// Utility methods and function objects in anonymous namespace
//-------------------------------------------------------------------------
/**
\class PIndex
\author S. Ansell
\date Aug 2005
\version 1.0
\brief Class  to fill an index with a progressive count
*/
template <typename T> struct PIndex {
private:
  int count; ///< counter
public:
  /// Constructor
  PIndex() : count(0) {}
  /// functional
  std::pair<T, int> operator()(const T &A) {
    return std::pair<T, int>(A, count++);
  }
};

/**
\class PSep
\author S. Ansell
\date Aug 2005
\version 1.0
\brief Class to access the second object in index pair.
*/
template <typename T> struct PSep {
  /// Functional to the second object
  int operator()(const std::pair<T, int> &A) { return A.second; }
};

/**
* Function to take a vector and sort the vector
* so as to produce an index. Leaves the vector unchanged.
* @param pVec :: Input vector
* @param Index :: Output vector
*/
template <typename T>
void indexSort(const std::vector<T> &pVec, std::vector<int> &Index) {
  Index.resize(pVec.size());
  std::vector<typename std::pair<T, int>> PartList;
  PartList.resize(pVec.size());

  transform(pVec.begin(), pVec.end(), PartList.begin(), PIndex<T>());
  sort(PartList.begin(), PartList.end());
  transform(PartList.begin(), PartList.end(), Index.begin(), PSep<T>());
}
template void indexSort(const std::vector<double> &, std::vector<int> &);
template void indexSort(const std::vector<float> &, std::vector<int> &);
template void indexSort(const std::vector<int> &, std::vector<int> &);
} // End of Anonymous name space

template <typename T> std::vector<T> Matrix<T>::getVector() const {
  std::vector<T> rez(m_numRowsX * m_numColumnsY);
  size_t ic(0);
  for (size_t i = 0; i < m_numRowsX; i++) {
    for (size_t j = 0; j < m_numColumnsY; j++) {
      rez[ic] = m_rawData[i][j];
      ic++;
    }
  }
  return rez;
}
//
template <typename T>
Matrix<T>::Matrix(const size_t nrow, const size_t ncol, const bool makeIdentity)
    : m_numRowsX(0), m_numColumnsY(0)
/**
  Constructor with pre-set sizes. Matrix is zeroed
  @param nrow :: number of rows
  @param ncol :: number of columns
  @param makeIdentity :: flag for the constructor to return an identity matrix
*/
{
  // Note:: m_numRowsX, m_numColumnsY zeroed so setMem always works
  setMem(nrow, ncol);
  zeroMatrix();
  if (makeIdentity)
    identityMatrix();
}

template <typename T>
Matrix<T>::Matrix(const std::vector<T> &A, const std::vector<T> &B)
    : m_numRowsX(0), m_numColumnsY(0)
/**
  Constructor to take two vectors and multiply them to
  construct a matrix. (assuming that we have columns x row
  vector.
  @param A :: Column vector to multiply
  @param B :: Row vector to multiply
*/
{
  // Note:: m_numRowsX,m_numColumnsY zeroed so setMem always works
  setMem(A.size(), B.size());
  for (size_t i = 0; i < m_numRowsX; i++) {
    for (size_t j = 0; j < m_numColumnsY; j++) {
      m_rawData[i][j] = A[i] * B[j];
    }
  }
}
//
template <typename T>
Matrix<T>::Matrix(const std::vector<T> &data)
    : m_numRowsX(0), m_numColumnsY(0) {
  size_t numel = data.size();
  size_t m_numRowsXt = static_cast<size_t>(sqrt(double(numel)));
  size_t test = m_numRowsXt * m_numRowsXt;
  if (test != numel) {
    throw(std::invalid_argument(
        "number of elements in input vector have to be square of some value"));
  }

  setMem(m_numRowsXt, m_numRowsXt);

  size_t ic(0);
  for (size_t i = 0; i < m_numRowsX; i++) {
    for (size_t j = 0; j < m_numColumnsY; j++) {
      m_rawData[i][j] = data[ic];
      ic++;
    }
  }
}

template <typename T>
Matrix<T>::Matrix(const std::vector<T> &data, const size_t nrow,
                  const size_t ncol)
    : m_numRowsX(0), m_numColumnsY(0) {
  size_t numel = data.size();
  size_t test = nrow * ncol;
  if (test != numel) {
    throw(std::invalid_argument("number of elements in input vector have is "
                                "incompatible with the number of rows and "
                                "columns"));
  }

  setMem(nrow, ncol);

  size_t ic(0);
  for (size_t i = 0; i < m_numRowsX; i++) {
    for (size_t j = 0; j < m_numColumnsY; j++) {
      m_rawData[i][j] = data[ic];
      ic++;
    }
  }
}

template <typename T>
Matrix<T>::Matrix(const Matrix<T> &A, const size_t nrow, const size_t ncol)
    : m_numRowsX(A.m_numRowsX - 1), m_numColumnsY(A.m_numColumnsY - 1)
/**
  Constructor with for a missing row/column.
  @param A :: The input matrix
  @param nrow :: number of row to miss
  @param ncol :: number of column to miss
*/
{
  if (nrow > m_numRowsX)
    throw Kernel::Exception::IndexError(nrow, A.m_numRowsX,
                                        "Matrix::Constructor without col");
  if (ncol > m_numColumnsY)
    throw Kernel::Exception::IndexError(ncol, A.m_numColumnsY,
                                        "Matrix::Constructor without col");
  setMem(m_numRowsX, m_numColumnsY);
  size_t iR(0);
  for (size_t i = 0; i <= m_numRowsX; i++) {
    if (i != nrow) {
      size_t jR(0);
      for (size_t j = 0; j <= m_numColumnsY; j++) {
        if (j != ncol) {

          m_rawData[iR][jR] = A.m_rawData[i][j];
          jR++;
        }
      }
      iR++;
    }
  }
}

template <typename T>
Matrix<T>::Matrix(const Matrix<T> &A)
    : m_numRowsX(0), m_numColumnsY(0)
/**
  Simple copy constructor
  @param A :: Object to copy
*/
{
  // Note:: m_numRowsX,m_numColumnsY zeroed so setMem always works
  setMem(A.m_numRowsX, A.m_numColumnsY);
  if (m_numRowsX * m_numColumnsY) {
    for (size_t i = 0; i < m_numRowsX; i++) {
      for (size_t j = 0; j < m_numColumnsY; j++) {
        m_rawData[i][j] = A.m_rawData[i][j];
      }
    }
  }
}

template <typename T>
Matrix<T> &Matrix<T>::operator=(const Matrix<T> &A)
/**
  Simple assignment operator
  @param A :: Object to copy
  @return the copied object
*/
{
  if (&A != this) {
    setMem(A.m_numRowsX, A.m_numColumnsY);
    if (m_numRowsX * m_numColumnsY) {
      for (size_t i = 0; i < m_numRowsX; i++) {
        for (size_t j = 0; j < m_numColumnsY; j++) {
          m_rawData[i][j] = A.m_rawData[i][j];
        }
      }
    }
  }
  return *this;
}

template <typename T>
Matrix<T>::Matrix(Matrix<T> &&other) noexcept
    : m_numRowsX(other.m_numRowsX),
      m_numColumnsY(other.m_numColumnsY),
      m_rawData(std::move(other.m_rawData)),
	  m_rawDataAlloc(std::move(other.m_rawDataAlloc)){
  other.m_numRowsX = 0;
  other.m_numColumnsY = 0;
}

template <typename T>
Matrix<T> &Matrix<T>::operator=(Matrix<T> &&other) noexcept {
  m_numRowsX = other.m_numRowsX;
  m_numColumnsY = other.m_numColumnsY;
  m_rawData = std::move(other.m_rawData);
  m_rawDataAlloc = std::move(other.m_rawDataAlloc);

  other.m_numRowsX = 0;
  other.m_numColumnsY = 0;

  return *this;
}

template <typename T>
Matrix<T> &Matrix<T>::operator+=(const Matrix<T> &A)
/**
  Matrix addition THIS + A
  If the size is different then 0 is added where appropiate
  Matrix A is not expanded.
  @param A :: Matrix to add
  @return Matrix(this + A)
*/
{
  const size_t Xpt((m_numRowsX > A.m_numRowsX) ? A.m_numRowsX : m_numRowsX);
  const size_t Ypt((m_numColumnsY > A.m_numColumnsY) ? A.m_numColumnsY
                                                     : m_numColumnsY);
  for (size_t i = 0; i < Xpt; i++) {
    for (size_t j = 0; j < Ypt; j++) {
      m_rawData[i][j] += A.m_rawData[i][j];
    }
  }

  return *this;
}

template <typename T>
Matrix<T> &Matrix<T>::operator-=(const Matrix<T> &A)
/**
  Matrix subtractoin THIS - A
  If the size is different then 0 is added where appropiate
  Matrix A is not expanded.
  @param A :: Matrix to add
  @return Ma
*/
{
  const size_t Xpt((m_numRowsX > A.m_numRowsX) ? A.m_numRowsX : m_numRowsX);
  const size_t Ypt((m_numColumnsY > A.m_numColumnsY) ? A.m_numColumnsY
                                                     : m_numColumnsY);
  for (size_t i = 0; i < Xpt; i++) {
    for (size_t j = 0; j < Ypt; j++) {
      m_rawData[i][j] -= A.m_rawData[i][j];
    }
  }

  return *this;
}

template <typename T>
Matrix<T> Matrix<T>::operator+(const Matrix<T> &A) const
/**
  Matrix addition THIS + A
  If the size is different then 0 is added where appropiate
  Matrix A is not expanded.
  @param A :: Matrix to add
  @return Matrix(this + A)
*/
{
  Matrix<T> X(*this);
  return X += A;
}

template <typename T>
Matrix<T> Matrix<T>::operator-(const Matrix<T> &A) const
/**
  Matrix subtraction THIS - A
  If the size is different then 0 is subtracted where
  appropiate. This matrix determines the size
  @param A :: Matrix to add
  @return Matrix(this + A)
*/
{
  Matrix<T> X(*this);
  return X -= A;
}

template <typename T>
Matrix<T> Matrix<T>::operator*(const Matrix<T> &A) const
/**
  Matrix multiplication THIS * A
  @param A :: Matrix to multiply by  (this->row must == A->columns)
  @throw MisMatch<size_t> if there is a size mismatch.
  @return Matrix(This * A)
*/
{
  if (m_numColumnsY != A.m_numRowsX)
    throw Kernel::Exception::MisMatch<size_t>(m_numColumnsY, A.m_numRowsX,
                                              "Matrix::operator*(Matrix)");
  Matrix<T> X(m_numRowsX, A.m_numColumnsY);
  for (size_t i = 0; i < m_numRowsX; i++) {
    for (size_t j = 0; j < A.m_numColumnsY; j++) {
      for (size_t kk = 0; kk < m_numColumnsY; kk++) {
        X.m_rawData[i][j] += m_rawData[i][kk] * A.m_rawData[kk][j];
      }
    }
  }
  return X;
}

template <typename T>
std::vector<T> Matrix<T>::operator*(const std::vector<T> &Vec) const
/**
  Matrix multiplication THIS * Vec to produce a vec
  @param Vec :: size of vector > this->nrows
  @throw MisMatch<size_t> if there is a size mismatch.
  @return Matrix(This * Vec)
*/
{
  if (m_numColumnsY > Vec.size())
    throw Kernel::Exception::MisMatch<size_t>(m_numColumnsY, Vec.size(),
                                              "Matrix::operator*(m_rawDataec)");

  std::vector<T> Out(m_numRowsX);
  for (size_t i = 0; i < m_numRowsX; i++) {
    for (size_t j = 0; j < m_numColumnsY; j++) {
      Out[i] += m_rawData[i][j] * Vec[j];
    }
  }
  return Out;
}

/**
  Matrix multiplication THIS * Vec to produce a vec
  @param in :: size of vector > this->nrows
  @param out :: result of Matrix(This * Vec)
  @throw MisMatch<size_t> if there is a size mismatch.
*/
template <typename T>
void Matrix<T>::multiplyPoint(const std::vector<T> &in,
                              std::vector<T> &out) const {
  out.resize(m_numRowsX);
  std::fill(std::begin(out), std::end(out), static_cast<T>(0.0));
  if (m_numColumnsY > in.size())
    throw Kernel::Exception::MisMatch<size_t>(m_numColumnsY, in.size(),
                                              "Matrix::multiplyPoint(in,out)");
  for (size_t i = 0; i < m_numRowsX; i++) {
    for (size_t j = 0; j < m_numColumnsY; j++) {
      out[i] += m_rawData[i][j] * in[j];
    }
  }
}

template <typename T>
V3D Matrix<T>::operator*(const V3D &Vx) const
/**
  Matrix multiplication THIS * V
  @param Vx :: Colunm vector to multiply by
  @throw MisMatch<size_t> if there is a size mismatch.
  @return Matrix(This * A)
*/
{
  if (m_numColumnsY != 3 || m_numRowsX > 3)
    throw Kernel::Exception::MisMatch<size_t>(m_numColumnsY, 3,
                                              "Matrix::operator*(m_rawData3D)");

  V3D v;
  for (size_t i = 0; i < m_numRowsX; ++i) {
    v[i] = m_rawData[i][0] * Vx.X() + m_rawData[i][1] * Vx.Y() +
           m_rawData[i][2] * Vx.Z();
  }

  return v;
}

template <typename T>
Matrix<T> Matrix<T>::operator*(const T &Value) const
/**
  Matrix multiplication THIS * Value
  @param Value :: Scalar to multiply by
  @return V * (this)
*/
{
  Matrix<T> X(*this);
  for (size_t i = 0; i < m_numRowsX; i++) {
    for (size_t j = 0; j < m_numColumnsY; j++) {
      X.m_rawData[i][j] *= Value;
    }
  }
  return X;
}

/**
  Matrix multiplication THIS *= A
  Note that we call operator* to avoid the problem
  of changing matrix size.
 @param A :: Matrix to multiply by  (this->row must == A->columns)
 @return This *= A
*/
template <typename T> Matrix<T> &Matrix<T>::operator*=(const Matrix<T> &A) {
  if (m_numColumnsY != A.m_numRowsX)
    throw Kernel::Exception::MisMatch<size_t>(m_numColumnsY, A.m_numRowsX,
                                              "Matrix*=(Matrix<T>)");
  // This construct to avoid the problem of changing size
  *this = this->operator*(A);
  return *this;
}

template <typename T>
Matrix<T> &Matrix<T>::operator*=(const T &Value)
/**
  Matrix multiplication THIS * Value
  @param Value :: Scalar to multiply matrix by
  @return *this
*/
{
  for (size_t i = 0; i < m_numRowsX; i++) {
    for (size_t j = 0; j < m_numColumnsY; j++) {
      m_rawData[i][j] *= Value;
    }
  }
  return *this;
}

template <typename T>
Matrix<T> &Matrix<T>::operator/=(const T &Value)
/**
  Matrix divishio THIS / Value
  @param Value :: Scalar to multiply matrix by
  @return *this
*/
{
  for (size_t i = 0; i < m_numRowsX; i++) {
    for (size_t j = 0; j < m_numColumnsY; j++) {
      m_rawData[i][j] /= Value;
    }
  }
  return *this;
}

template <typename T>
bool Matrix<T>::operator!=(const Matrix<T> &A) const
/**
Element by Element comparison
@param A :: Matrix to check
@return true :: on succees
@return false :: failure
*/
{
  return !(this->operator==(A));
}

template <typename T>
bool Matrix<T>::operator==(const Matrix<T> &A) const
/**
Element by element comparison within tolerance.
Tolerance means that the value must be > tolerance
and less than (diff/max)>tolerance

Always returns 0 if the Matrix have different sizes
@param A :: matrix to check.
@return true on success
*/
{
  return this->equals(A, 1e-8);
}

//---------------------------------------------------------------------------------------
template <typename T>
bool Matrix<T>::equals(const Matrix<T> &A, const double Tolerance) const
/**
Element by element comparison within tolerance.
Tolerance means that the value must be > tolerance
and less than (diff/max)>tolerance

Always returns 0 if the Matrix have different sizes
@param A :: matrix to check.
@param Tolerance :: tolerance in comparing elements
@return true on success
*/
{
  if (&A != this) // this == A == always true
  {
    if (A.m_numRowsX != m_numRowsX || A.m_numColumnsY != m_numColumnsY)
      return false;

    double maxS(0.0);
    double maxDiff(0.0); // max di
    for (size_t i = 0; i < m_numRowsX; i++)
      for (size_t j = 0; j < m_numColumnsY; j++) {
        const T diff = (m_rawData[i][j] - A.m_rawData[i][j]);
        if (fabs(diff) > maxDiff)
          maxDiff = fabs(diff);
        if (fabs(m_rawData[i][j]) > maxS)
          maxS = fabs(m_rawData[i][j]);
      }
    if (maxDiff < Tolerance)
      return true;
    if (maxS > 1.0 && (maxDiff / maxS) < Tolerance)
      return true;
    return false;
  }
  // this == this is true
  return true;
}

//---------------------------------------------------------------------------------------
/** Element by element comparison of <
Always returns false if the Matrix have different sizes
@param A :: matrix to check.
@return true if this < A
*/
template <typename T> bool Matrix<T>::operator<(const Matrix<T> &A) const {
  if (&A == this) // this < A == always false
    return false;

  if (A.m_numRowsX != m_numRowsX || A.m_numColumnsY != m_numColumnsY)
    return false;

  for (size_t i = 0; i < m_numRowsX; i++)
    for (size_t j = 0; j < m_numColumnsY; j++) {
      if (m_rawData[i][j] >= A.m_rawData[i][j])
        return false;
    }
  return true;
}

//---------------------------------------------------------------------------------------
/** Element by element comparison of >=
Always returns false if the Matrix have different sizes
@param A :: matrix to check.
@return true if this >= A
*/
template <typename T> bool Matrix<T>::operator>=(const Matrix<T> &A) const {
  if (&A == this)
    return true;

  if (A.m_numRowsX != m_numRowsX || A.m_numColumnsY != m_numColumnsY)
    return false;

  for (size_t i = 0; i < m_numRowsX; i++)
    for (size_t j = 0; j < m_numColumnsY; j++) {
      if (m_rawData[i][j] < A.m_rawData[i][j])
        return false;
    }
  return true;
}

/**
  Sets the memory held in matrix
  @param a :: number of rows
  @param b :: number of columns
*/
template <typename T> void Matrix<T>::setMem(const size_t a, const size_t b) {
	if (a == m_numRowsX && b == m_numColumnsY && m_rawData != nullptr)
	return;

  if (a <= 0 || b <= 0)
    return;

  m_numRowsX = a;
  m_numColumnsY = b;

  // Allocate memory first - this has to be a flat 1d array masquerading as a 2d array
  // so we can expose the memory to Python APIs via numpy which expects this 
  // style of memory layout.

  // Note: Don't change these to a 'auto'. By strongly typing here we know
  // that the move at the end can be done (and we get nice clean compiler
  // error messages at this point if it cannot).
  CMemoryArray<T> allocatedMemory = std::make_unique <T[]> ((m_numRowsX * m_numColumnsY));

  // Next allocate an array of pointers for the rows (X). This partitions
  // the 1D array into a 2D array for callers. 
  MatrixMemoryPtrs<T> rowPtrs = std::make_unique<T*[]>(m_numRowsX);

  for (size_t i = 0; i < m_numRowsX; i++) {
	  // Calculate offsets into the allocated memory array (Y)
	  rowPtrs[i] = &allocatedMemory[i * m_numColumnsY];
  }

  m_rawDataAlloc = std::move(allocatedMemory);
  m_rawData = std::move(rowPtrs);
}

/**
  Swap rows I and J
  @param RowI :: row I to swap
  @param RowJ :: row J to swap
*/
template <typename T>
void Matrix<T>::swapRows(const size_t RowI, const size_t RowJ) {
  if (m_numRowsX * m_numColumnsY && RowI < m_numRowsX && RowJ < m_numRowsX &&
      RowI != RowJ) {
    for (size_t k = 0; k < m_numColumnsY; k++) {
      T tmp = m_rawData[RowI][k];
      m_rawData[RowI][k] = m_rawData[RowJ][k];
      m_rawData[RowJ][k] = tmp;
    }
  }
}

/**
  Swap columns I and J
  @param colI :: col I to swap
  @param colJ :: col J to swap
*/
template <typename T>
void Matrix<T>::swapCols(const size_t colI, const size_t colJ) {
  if (m_numRowsX * m_numColumnsY && colI < m_numColumnsY &&
      colJ < m_numColumnsY && colI != colJ) {
    for (size_t k = 0; k < m_numRowsX; k++) {
      T tmp = m_rawData[k][colI];
      m_rawData[k][colI] = m_rawData[k][colJ];
      m_rawData[k][colJ] = tmp;
    }
  }
}

template <typename T>
void Matrix<T>::zeroMatrix()
/**
  Zeros all elements of the matrix
*/
{
  if (m_numRowsX * m_numColumnsY) {
    for (size_t i = 0; i < m_numRowsX; i++) {
      for (size_t j = 0; j < m_numColumnsY; j++) {
        m_rawData[i][j] = static_cast<T>(0);
      }
    }
  }
}

template <typename T>
void Matrix<T>::identityMatrix()
/**
  Makes the matrix an idenity matrix.
  Zeros all the terms outside of the square
*/
{
  if (m_numRowsX * m_numColumnsY) {
    for (size_t i = 0; i < m_numRowsX; i++) {
      for (size_t j = 0; j < m_numColumnsY; j++) {
        m_rawData[i][j] = static_cast<T>(j == i);
      }
    }
  }
}
template <typename T>
void Matrix<T>::setColumn(const size_t nCol, const std::vector<T> &newCol) {
  if (nCol >= this->m_numColumnsY) {
    throw(std::invalid_argument("nCol requested> nCol availible"));
  }
  size_t m_numRowsXM = newCol.size();
  if (m_numRowsX < m_numRowsXM)
    m_numRowsXM = m_numRowsX;
  for (size_t i = 0; i < m_numRowsXM; i++) {
    m_rawData[i][nCol] = newCol[i];
  }
}
template <typename T>
void Matrix<T>::setRow(const size_t nRow, const std::vector<T> &newRow) {
  if (nRow >= this->m_numRowsX) {
    throw(std::invalid_argument("nRow requested> nRow availible"));
  }
  size_t m_numColumnsYM = newRow.size();
  if (m_numColumnsY < m_numColumnsYM)
    m_numColumnsYM = m_numColumnsY;
  for (size_t j = 0; j < m_numColumnsYM; j++) {
    m_rawData[nRow][j] = newRow[j];
  }
}

template <typename T>
void Matrix<T>::rotate(const double tau, const double s, const int i,
                       const int j, const int k, const int m)
/**
  Applies a rotation to a particular point of tan(theta)=tau.
  Note that you need both sin(theta) and tan(theta) because of
  sign preservation.
  @param tau :: tan(theta)
  @param s :: sin(theta)
  @param i ::  first index (xpos)
  @param j ::  first index (ypos)
  @param k ::  second index (xpos)
  @param m ::  second index (ypos)
 */
{
  const T gg = m_rawData[i][j];
  const T hh = m_rawData[k][m];
  m_rawData[i][j] = static_cast<T>(gg - s * (hh + gg * tau));
  m_rawData[k][m] = static_cast<T>(hh + s * (gg - hh * tau));
}

template <typename T>
Matrix<T> Matrix<T>::preMultiplyByDiagonal(const std::vector<T> &Dvec) const
/**
  Creates a diagonal matrix D from the given vector Dvec and
  PRE-multiplies the matrix by it (i.e. D * M).
  @param Dvec :: diagonal matrix (just centre points)
  @return D*this
*/
{
  if (Dvec.size() != m_numRowsX) {
    std::ostringstream cx;
    cx << "Matrix::preMultiplyByDiagonal Size: " << Dvec.size() << " "
       << m_numRowsX << " " << m_numColumnsY;
    throw std::runtime_error(cx.str());
  }
  Matrix<T> X(Dvec.size(), m_numColumnsY);
  for (size_t i = 0; i < Dvec.size(); i++) {
    for (size_t j = 0; j < m_numColumnsY; j++) {
      X.m_rawData[i][j] = Dvec[i] * m_rawData[i][j];
    }
  }
  return X;
}

template <typename T>
Matrix<T> Matrix<T>::postMultiplyByDiagonal(const std::vector<T> &Dvec) const
/**
  Creates a diagonal matrix D from the given vector Dvec and
  POST-multiplies the matrix by it (i.e. M * D).
  @param Dvec :: diagonal matrix (just centre points)
  @return this*D
*/
{
  if (Dvec.size() != m_numColumnsY) {
    std::ostringstream cx;
    cx << "Error Matrix::bDiaognal size:: " << Dvec.size() << " " << m_numRowsX
       << " " << m_numColumnsY;
    throw std::runtime_error(cx.str());
  }

  Matrix<T> X(m_numRowsX, Dvec.size());
  for (size_t i = 0; i < m_numRowsX; i++) {
    for (size_t j = 0; j < Dvec.size(); j++) {
      X.m_rawData[i][j] = Dvec[j] * m_rawData[i][j];
    }
  }
  return X;
}

template <typename T>
Matrix<T> Matrix<T>::Tprime() const
/**
  Transpose the matrix :
  Has transpose for a square matrix case.
  @return M^T
*/
{
  if (!m_numRowsX * m_numColumnsY)
    return *this;

  if (m_numRowsX == m_numColumnsY) // inplace transpose
  {
    Matrix<T> MT(*this);
    MT.Transpose();
    return MT;
  }

  // irregular matrix
  Matrix<T> MT(m_numColumnsY, m_numRowsX);
  for (size_t i = 0; i < m_numRowsX; i++)
    for (size_t j = 0; j < m_numColumnsY; j++)
      MT.m_rawData[j][i] = m_rawData[i][j];

  return MT;
}

template <typename T>
Matrix<T> &Matrix<T>::Transpose()
/**
Transpose the matrix :
Has a in place transpose for a square matrix case.
@return this^T
*/
{
	if (!m_numRowsX * m_numColumnsY)
		return *this;

	if (m_numRowsX == m_numColumnsY) // in place transpose
	{
		for (size_t i = 0; i < m_numRowsX; i++) {
			for (size_t j = i + 1; j < m_numColumnsY; j++) {
				std::swap(m_rawData[i][j], m_rawData[j][i]);
			}
		}
		return *this;
	}

	// irregular matrix
	// get some memory

	// Note: Don't change these to a 'auto'. By strongly typing here we know
	// that the move at the end can be done (and we get nice clean compiler
	// error messages at this point if it cannot).
	CMemoryArray<T> allocatedMemory = std::make_unique <T[]>((m_numRowsX * m_numColumnsY));
	MatrixMemoryPtrs<T> transposePtrs = std::make_unique<T*[]>(m_numRowsX);

	for (size_t i = 0; i < m_numColumnsY; i++) {
		// Notice how this partitions using Rows (X) instead of Cols(Y)
		transposePtrs[i] = &allocatedMemory[i * m_numRowsX];
	}

	for (size_t i = 0; i < m_numRowsX; i++) {
		for (size_t j = 0; j < m_numColumnsY; j++) {
			transposePtrs[j][i] = m_rawData[i][j];
		}
	}
	// remove old memory
	std::swap(m_numRowsX, m_numColumnsY);
	
	m_rawDataAlloc = std::move(allocatedMemory);
	m_rawData = std::move(transposePtrs);

	return *this;
}

template <>
void Matrix<int>::GaussJordan(Kernel::Matrix<int> &)
/**
  Not valid for Integer
  @throw std::invalid_argument
*/
{
  throw std::invalid_argument(
      "Gauss-Jordan inversion not valid for integer matrix");
}

template <typename T>
void Matrix<T>::GaussJordan(Matrix<T> &B)
/**
  Invert this matrix in place using Gauss-Jordan elimination.
  Matrix will be replaced by its inverse.
  @param B :: [input, output] Must have same dimensions as A. Returned as
  identity matrix. (?)
  @throw std::invalid_argument on input error
  @throw std::runtime_error if singular
 */
{
  // check for input errors
  if (m_numRowsX != m_numColumnsY || B.m_numRowsX != m_numRowsX) {
    throw std::invalid_argument("Matrix not square, or sizes do not match");
  }

  // pivoted rows
  std::vector<int> pivoted(m_numRowsX);
  fill(pivoted.begin(), pivoted.end(), 0);

  std::vector<int> indxcol(m_numRowsX); // Column index
  std::vector<int> indxrow(m_numRowsX); // row index

  size_t irow(0), icol(0);
  for (size_t i = 0; i < m_numRowsX; i++) {
    // Get Biggest non-pivoted item
    double bigItem = 0.0; // get point to pivot over
    for (size_t j = 0; j < m_numRowsX; j++) {
      if (pivoted[j] != 1) // check only non-pivots
      {
        for (size_t k = 0; k < m_numRowsX; k++) {
          if (!pivoted[k]) {
            if (fabs(m_rawData[j][k]) >= bigItem) {
              bigItem = fabs(m_rawData[j][k]);
              irow = j;
              icol = k;
            }
          }
        }
      } else if (pivoted[j] > 1) {
        throw std::runtime_error("Error doing G-J elem on a singular matrix");
      }
    }
    pivoted[icol]++;
    // Swap in row/col to make a diagonal item
    if (irow != icol) {
      swapRows(irow, icol);
      B.swapRows(irow, icol);
    }
    indxrow[i] = static_cast<int>(irow);
    indxcol[i] = static_cast<int>(icol);

    if (m_rawData[icol][icol] == 0.0) {
      throw std::runtime_error("Error doing G-J elem on a singular matrix");
    }
    const T pivDiv = T(1.0) / m_rawData[icol][icol];
    m_rawData[icol][icol] = 1;
    for (size_t l = 0; l < m_numRowsX; l++) {
      m_rawData[icol][l] *= pivDiv;
    }
    for (size_t l = 0; l < B.m_numColumnsY; l++) {
      B.m_rawData[icol][l] *= pivDiv;
    }

    for (size_t ll = 0; ll < m_numRowsX; ll++) {
      if (ll != icol) {
        const T div_num = m_rawData[ll][icol];
        m_rawData[ll][icol] = 0.0;
        for (size_t l = 0; l < m_numRowsX; l++) {
          m_rawData[ll][l] -= m_rawData[icol][l] * div_num;
        }
        for (size_t l = 0; l < B.m_numColumnsY; l++) {
          B.m_rawData[ll][l] -= B.m_rawData[icol][l] * div_num;
        }
      }
    }
  }

  // Un-roll interchanges
  if (m_numRowsX > 0) {
    for (int l = static_cast<int>(m_numRowsX) - 1; l >= 0; l--) {
      if (indxrow[l] != indxcol[l]) {
        swapCols(indxrow[l], indxcol[l]);
      }
    }
  }
}

template <typename T>
T Matrix<T>::Invert()
/**
  If the Matrix is square then invert the matrix
  using LU decomposition
  @return Determinant (0 if the matrix is singular)
*/
{
  if (m_numRowsX != m_numColumnsY && m_numRowsX < 1)
    return 0;

  if (m_numRowsX == 1) {
    T det = m_rawData[0][0];
    if (m_rawData[0][0] != static_cast<T>(0.))
      m_rawData[0][0] = static_cast<T>(1.) / m_rawData[0][0];
    return det;
  }
  auto indx = new int[m_numRowsX]; // Set in lubcmp

  auto col = new double[m_numRowsX];
  int d;
  Matrix<T> Lcomp(*this);
  Lcomp.lubcmp(indx, d);

  double det = static_cast<double>(d);
  for (size_t j = 0; j < m_numRowsX; j++)
    det *= Lcomp.m_rawData[j][j];

  for (size_t j = 0; j < m_numRowsX; j++) {
    for (size_t i = 0; i < m_numRowsX; i++)