#include "Matrix.h"
#if !defined(NO_STD)
#include <math.h>
#include <iostream>

#endif

namespace LinearAlgebra {

#pragma region Matrix1Of

template <typename T>
Matrix1Of<T>::Matrix1Of() {}

template <typename T>
Matrix1Of<T>::Matrix1Of(int size) : size(size), externalData(false) {
  if (this->size <= 0)
    data = nullptr;
  else
    this->data = new T[size]();
}

template <typename T>
Matrix1Of<T>::Matrix1Of(int size, T* data) : data(data), size(size) {
  this->externalData = true;
}

template <typename T>
Matrix1Of<T>::Matrix1Of(const Matrix1Of& m) : size(m.size) {
  if (this->size <= 0)
    this->data = nullptr;
  else {
    this->data = new T[this->size];

    for (unsigned int ix = 0; ix < this->size; ++ix)
      this->data[ix] = m.data[ix];
  }
}

template <typename T>
Matrix1Of<T>::Matrix1Of(Matrix1Of&& m) noexcept : data(m.data), size(m.size) {
  m.data = nullptr;  // Leave the moved-from object in a safe state
  m.size = 0;
}

template <typename T>
Matrix1Of<T>::~Matrix1Of() {
  if (!this->externalData) {
    if (this->size > 0)
      delete[] data;
  }
}

template <typename T>
Matrix1Of<T>& Matrix1Of<T>::operator=(const Matrix1Of& m) {
  if (this != &m) {
    if (this->size > 0)
      delete[] this->data;  // Free the current memory

    this->size = m.size;
    this->externalData = m.externalData;
    if (this->size == 0)
      this->data = nullptr;
    else {
      this->data = new T[this->size];
      for (unsigned int ix = 0; ix < this->size; ++ix)
        this->data[ix] = m.data[ix];
    }
  }
  return *this;
}

template <typename T>
Matrix1Of<T>& Matrix1Of<T>::operator=(Matrix1Of&& m) noexcept {
  if (this != &m) {
    if (this->size > 0) {
      // printf("Matrix1 Move Assignment Deallocate %d\n", this->size);
      delete[] this->data;  // Free the current memory
    }

    // Transfer ownership of resources
    this->size = m.size;
    this->data = m.data;
    this->externalData = m.externalData;

    // Leave the moved-from object in a valid state
    m.size = 0;
    m.data = nullptr;
  }
  return *this;
}

template <typename T>
unsigned int Matrix1Of<T>::Size() const {
  return this->size;
}

template <typename T>
void Matrix1Of<T>::Resize(int size) {
  if (!this->externalData && this->size > 0)
    delete[] this->data;

  this->externalData = false;

  if (size <= 0) {
    this->data = nullptr;
    this->size = 0;
  } else {
    this->data = new T[size]();
  }
}

template <typename T>
Matrix1Of<T> Matrix1Of<T>::Zero(unsigned int size) {
  Matrix1Of<T> r(size);
  r.Fill(0);
  return r;
}

template <typename T>
void Matrix1Of<T>::Fill(T value) {
  for (unsigned int ix = 0; ix < this->size; ix++)
    this->data[ix] = value;
}

template <typename T>
void Matrix1Of<T>::Fill(T value, unsigned int start, unsigned int stop) {
  if (start > stop || start > this->size)
    return;

  if (stop > this->size)
    stop = this->size;

  for (unsigned int ix = start; ix < stop; ix++)
    this->data[ix] = value;
}

template <typename T>
Matrix1Of<T> Matrix1Of<T>::FromVector3(Vector3Of<T> v) {
  Matrix1Of<T> r(3);
  r.data[0] = v.horizontal;
  r.data[1] = v.vertical;
  r.data[2] = v.depth;
  return r;
}

template <typename T>
Vector3Of<T> Matrix1Of<T>::ToVector3() {
  return Vector3Of<T>(this->data[0], this->data[1], this->data[2]);
}

template <typename T>
Matrix1Of<float> Matrix1Of<T>::FromQuaternion(Quaternion q) {
  Matrix1Of<float> r(4);
  r(0) = q.x;
  r(1) = q.y;
  r(2) = q.z;
  r(3) = q.w;
  return r;
}

template <typename T>
Quaternion Matrix1Of<T>::ToQuaternion() {
    float x = static_cast<float>(data[0]);
    float y = static_cast<float>(data[1]);
    float z = static_cast<float>(data[2]);
    float w = static_cast<float>(data[3]);
    return Quaternion(x, y, z, w);
}

template <typename T>
Matrix1Of<T> Matrix1Of<T>::Slice(unsigned int start, unsigned int stop) {
  if (start < 0 || stop >= this->size)
    std::cerr << "Slice index out of range." << std::endl;

  Matrix1Of<T> result(stop - start);
  int resultIx = 0;
  for (unsigned int ix = start; ix < stop; ix++)
    result.data[resultIx++] = this->data[ix];

  return result;
}

template <typename T>
void Matrix1Of<T>::UpdateSlice(unsigned int start, unsigned int stop, const Matrix1Of& source) const {
  if (start > stop || start > this->size || start > source.size)
    return;

  if (stop > this->size)
    stop = this->size;
  if (stop > source.size)
    stop = source.size;

  unsigned int sourceIx = 0;
  for (unsigned int ix = start; ix < stop; ix++, sourceIx++) {
    this->data[ix] = source.data[sourceIx];
  }
}

template <typename T>
const T& Matrix1Of<T>::operator()(int ix) const {
  return data[ix];
}
template <typename T>
T& Matrix1Of<T>::operator()(int ix) {
  return data[ix];
}

template class Matrix1Of<int>;
template class Matrix1Of<float>;
template class Matrix1Of<double>;

#pragma endregion Matrix1Of

/*
#pragma region Matrix1

Matrix1::Matrix1() {}

Matrix1::Matrix1(int size) : size(size), externalData(false) {
  if (this->size <= 0)
    data = nullptr;
  else
    this->data = new float[size]();
}

Matrix1::Matrix1(int size, float* data) : data(data), size(size) {
  this->externalData = true;
}

Matrix1::Matrix1(const Matrix1& m) : size(m.size) {
  if (this->size <= 0)
    this->data = nullptr;
  else {
    this->data = new float[this->size];

    for (int ix = 0; ix < this->size; ++ix)
      this->data[ix] = m.data[ix];
  }
}

Matrix1::Matrix1(Matrix1&& m) noexcept : data(m.data), size(m.size) {
  m.data = nullptr;  // Leave the moved-from object in a safe state
  m.size = 0;
}

Matrix1::~Matrix1() {
  if (!this->externalData) {
    if (this->size > 0)
      delete[] data;
  }
}

Matrix1& Matrix1::operator=(const Matrix1& m) {
  if (this != &m) {
    if (this->size > 0)
      delete[] this->data;  // Free the current memory

    this->size = m.size;
    this->externalData = m.externalData;
    if (this->size == 0)
      this->data = nullptr;
    else {
      this->data = new float[this->size];
      for (int ix = 0; ix < this->size; ++ix)
        this->data[ix] = m.data[ix];
    }
  }
  return *this;
}

Matrix1& Matrix1::operator=(Matrix1&& m) noexcept {
  if (this != &m) {
    if (this->size > 0) {
      // printf("Matrix1 Move Assignment Deallocate %d\n", this->size);
      delete[] this->data;  // Free the current memory
    }

    // Transfer ownership of resources
    this->size = m.size;
    this->data = m.data;
    this->externalData = m.externalData;

    // Leave the moved-from object in a valid state
    m.size = 0;
    m.data = nullptr;
  }
  return *this;
}

unsigned int Matrix1::Size() const {
  return this->size;
}

void Matrix1::Resize(int size) {
  if (!this->externalData && this->size > 0)
    delete[] this->data;

  this->externalData = false;

  if (size <= 0) {
    this->data = nullptr;
    this->size = 0;
  } else {
    this->data = new float[size]();
  }
}

Matrix1 Matrix1::FromVector3(Vector3Float v) {
  Matrix1 r(3);
  r.data[0] = v.horizontal;
  r.data[1] = v.vertical;
  r.data[2] = v.depth;
  return r;
}

Matrix1 Matrix1::Zero(unsigned int size) {
  Matrix1 r(size);
  r.Fill(0);
  return r;
}

void Matrix1::Fill(float value) {
  for (int ix = 0; ix < this->size; ix++)
    this->data[ix] = value;
}

void Matrix1::Fill(float value, unsigned int start, unsigned int stop) {
  if (start > stop || start > this->size)
    return;

  if (stop > this->size)
    stop = this->size;

  for (unsigned int ix = start; ix < stop; ix++)
    this->data[ix] = value;
}

Vector3Float Matrix1::ToVector3() {
  return Vector3Float(this->data[0], this->data[1], this->data[2]);
}

Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) {
  Matrix1 r = Matrix1(4);
  float* data = r.data;
  data[0] = q.x;
  data[1] = q.y;
  data[2] = q.z;
  data[3] = q.w;
  return r;
}

Quaternion LinearAlgebra::Matrix1::ToQuaternion() {
  return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]);
}

Matrix1 Matrix1::Slice(int start, int stop) {
  if (start < 0 || stop >= this->size)
    std::cerr << "Slice index out of range." << std::endl;

  Matrix1 result(stop - start);
  int resultIx = 0;
  for (int ix = start; ix < stop; ix++)
    result.data[resultIx++] = this->data[ix];

  return result;
}

void Matrix1::UpdateSlice(int start, int stop, const Matrix1& source) const {
  if (start > stop || start > this->size || start > source.size)
    return;

  if (stop > this->size)
    stop = this->size;
  if (stop > source.size)
    stop = source.size;

  unsigned int sourceIx = 0;
  for (int ix = start; ix < stop; ix++, sourceIx++) {
    this->data[ix] = source.data[sourceIx];
  }
}

const float& Matrix1::operator()(int ix) const {
  return data[ix];
}
float& Matrix1::operator()(int ix) {
  return data[ix];
}

// Matrix1
#pragma endregion
*/

/*
#pragma region Matrix2

Matrix2::Matrix2() {}

Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols), externalData(false) {
  this->size = nRows * nCols;

  if (this->size <= 0) {
    this->data = nullptr;
    return;
  }

  // printf("Matrix2 Allocate %d\n", this->size * sizeof(float));
  this->data = new float[this->size];
}

Matrix2::Matrix2(int nRows, int nCols, float* data) : nRows(nRows), nCols(nCols), data(data) {
  // printf("Matrix2 No-Allocate %d\n", this->size * sizeof(float));
  this->size = nRows * nCols;
  this->externalData = true;
}

Matrix2::Matrix2(const Matrix2& m) : nRows(m.nRows), nCols(m.nCols), size(m.size) {
  // printf("Matrix2 Copy Allocate %d\n", this->size * sizeof(float));
  if (this->size == 0)
    this->data = nullptr;
  else {
    this->data = new float[this->size];

    for (int ix = 0; ix < this->size; ++ix)
      this->data[ix] = m.data[ix];
  }
}

Matrix2::Matrix2(Matrix2&& m) noexcept
    : nRows(m.nRows), nCols(m.nCols), size(m.size), data(m.data) {
  m.nRows = 0;
  m.nCols = 0;
  m.size = 0;
  m.data = nullptr;  // Leave the moved-from object in a safe state
}

Matrix2& Matrix2::operator=(const Matrix2& m) {
  if (this != &m) {
    if (this->size > 0) {
      // printf("Matrix2 Copy Assignment Deallocate %d\n", this->size * sizeof(float));
      delete[] this->data;  // Free the current memory
    }

    this->nRows = m.nRows;
    this->nCols = m.nCols;
    this->size = m.size;
    this->externalData = m.externalData;
    if (this->size == 0)
      this->data = nullptr;
    else {
      this->data = new float[this->size];
      for (int ix = 0; ix < this->size; ++ix)
        this->data[ix] = m.data[ix];
    }
  }
  return *this;
}

Matrix2& Matrix2::operator=(Matrix2&& m) noexcept {
  if (this != &m) {
    if (this->size > 0) {
      // printf("Matrix2 Move Assignment Deallocate %d\n", this->size * sizeof(float));
      delete[] this->data;  // Free the current memory
    }

    // Transfer ownership of resources
    this->nRows = m.nRows;
    this->nCols = m.nCols;
    this->size = m.size;
    this->externalData = m.externalData;
    this->data = m.data;

    // Leave the moved-from object in a valid state
    m.size = 0;
    m.data = nullptr;
  }
  return *this;
}

Matrix2::~Matrix2() {
  if (!this->externalData) {
    if (this->size > 0) {
      // printf("Matrix2 Deallocate %d\n", this->size * sizeof(float));
      delete[] data;
    }
  }
}

unsigned int Matrix2::NRows() const {
  return this->nRows;
}

unsigned int Matrix2::NCols() const {
  return this->nCols;
}

Matrix2 Matrix2::Zero(int nRows, int nCols) {
  Matrix2 r = Matrix2(nRows, nCols);
  for (int ix = 0; ix < r.size; ix++)
    r.data[ix] = 0;
  return r;
}

const float& Matrix2::operator()(int row, int col) const {
  return data[row * this->nCols + col];
}
float& Matrix2::operator()(int row, int col) {
  return data[row * this->nCols + col];
}

void Matrix2::Fill(float value) {
  for (int ix = 0; ix < this->size; ix++)
    this->data[ix] = value;
}

void Matrix2::FillDiagonal(float f) {
  int n = this->nRows < this->nCols ? nRows : nCols;
  for (int ix = 0; ix < n; ix++)
    this->data[ix * this->nCols + ix] = f;
}

Matrix2 Matrix2::Identity(int size) {
  return Diagonal(1, size);
}

Matrix2 Matrix2::Identity(int nRows, int nCols) {
  Matrix2 m = Matrix2::Zero(nRows, nCols);
  m.FillDiagonal(1);
  return m;
}

Matrix2 Matrix2::Diagonal(float f, int size) {
  Matrix2 r = Matrix2::Zero(size, size);
  float* data = r.data;
  int valueIx = 0;
  for (int ix = 0; ix < size; ix++) {
    data[valueIx] = f;
    valueIx += size + 1;
  }
  return r;
}

Matrix2 Matrix2::SkewMatrix(const Vector3Of<float>& v) {
  Matrix2 r = Matrix2(3, 3);
  float* data = r.data;
  data[0 * 3 + 0] = 0;
  data[0 * 3 + 1] = -v.depth;    // result(0, 1)
  data[0 * 3 + 2] = v.vertical;  // result(0, 2)
  data[1 * 3 + 0] = v.depth;     // result(1, 0)
  data[1 * 3 + 1] = 0;
  data[1 * 3 + 2] = -v.horizontal;  // result(1, 2)
  data[2 * 3 + 0] = -v.vertical;    // result(2, 0)
  data[2 * 3 + 1] = v.horizontal;   // result(2, 1)
  data[2 * 3 + 2] = 0;
  return r;
}

Matrix2 Matrix2::Transpose() const {
  Matrix2 r = Matrix2(this->nCols, this->nRows);

  for (int rowIx = 0; rowIx < this->nRows; rowIx++) {
    for (int colIx = 0; colIx < this->nCols; colIx++)
      r.data[colIx * this->nRows + rowIx] = this->data[rowIx * this->nCols + colIx];
  }
  return r;
}

Matrix2 LinearAlgebra::Matrix2::operator-() const {
  Matrix2 r = Matrix2(this->nRows, this->nCols);
  for (int ix = 0; ix < r.size; ix++)
    r.data[ix] = -this->data[ix];
  return r;
}

Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const {
  Matrix2 r = Matrix2(this->nRows, this->nCols);
  for (int ix = 0; ix < r.size; ix++)
    r.data[ix] = this->data[ix] + v.data[ix];
  return r;
}

Matrix2& Matrix2::operator+=(const Matrix2& v) {
  for (int ix = 0; ix < this->size; ix++)
    this->data[ix] += v.data[ix];
  return *this;
}

Matrix2 Matrix2::operator-(const Matrix2& v) const {
  Matrix2 r = Matrix2(this->nRows, this->nCols);
  for (int ix = 0; ix < r.size; ix++)
    r.data[ix] = this->data[ix] - v.data[ix];
  return r;
}

Matrix1 operator*(const Matrix2& m, const Matrix1& v) {
  Matrix1 r = Matrix1(m.nRows);
  for (int rowIx = 0; rowIx < m.nRows; rowIx++) {
    int mRowIx = rowIx * m.nCols;
    for (int colIx = 0; colIx < m.nCols; colIx++)
      r(rowIx) += m.data[mRowIx + colIx] * v(colIx);
    // r(rowIx) += m(rowIx, colIx) * v(colIx);
  }
  return r;
}

Matrix2 Matrix2::operator*(const Matrix2& B) const {
  Matrix2 r = Matrix2(this->nRows, B.nCols);

  const int ACols = this->nCols;
  const int BCols = B.nCols;
  const int ARows = this->nRows;
  // const int BRows = B.nRows;

  for (int i = 0; i < ARows; ++i) {
    // Pre-compute row offsets
    int ARowOffset = i * ACols;  // ARowOffset is constant for each row of A
    int BColOffset = i * BCols;  // BColOffset is constant for each row of B
    for (int j = 0; j < BCols; ++j) {
      float sum = 0;
      // printf(" 0");
      int BIndex = j;
      for (int k = 0; k < ACols; ++k) {
        // printf(" + %f * %f", this->data[ARowOffset + k], B.data[BIndex]);
        //  std::cout << " + " << this->data[ARowOffset + k] << " * "
        //            << B.data[BIndex];
        sum += this->data[ARowOffset + k] * B.data[BIndex];
        BIndex += BCols;
      }
      r.data[BColOffset + j] = sum;
      // std::cout << " = " << sum << " ix: " << BColOffset + j << "\n";
      // if (sum != std::trunc(sum)) {
      //   int BIndex = j;
      //   printf(" 0");
      //   for (int k = 0; k < ACols; ++k) {
      //     printf(" + %f * %f", this->data[ARowOffset + k], B.data[BIndex]);
      //     BIndex += BCols;
      //   }
      //   printf(" = %f ix: %d\n", sum, BColOffset + j);
      // }
    }
  }

  //  if (ACols != BRows)
  //   printf("### Acols != Nrows\n");

  //       for (size_t i = 0; i < ARows; ++i) {
  //         for (size_t p = 0; p < ACols; ++p) {
  //             float a_ip = this->data[i * ACols + p];           // A(i,p)
  //             for (size_t j = 0; j < BCols; ++j) {
  //                 r.data[i * BCols + j] += a_ip * B.data[p * BCols + j]; //
  //                 C(i,j) += A(i,p)*B(p,j)
  //             }
  //         }
  //     }

  return r;
}

Matrix2 Matrix2::GetRows(int from, int to) {
  if (from < 0 || to >= this->nRows)
    std::cerr << "Slice index out of range." << std::endl;

  Matrix2 result = Matrix2(to - from, this->nCols);
  int resultRowIx = 0;
  for (int rowIx = from; rowIx < to; rowIx++) {
    for (int colIx = 0; colIx < this->nCols; colIx++)
      result.data[resultRowIx * result.nCols + colIx] = this->data[rowIx * this->nCols + colIx];

    resultRowIx++;
  }

  return result;
}

Vector3 Matrix2::GetRow3(int rowIx) {
  int cellIx = rowIx * this->nCols;
  Vector3 row(this->data[cellIx + 0], this->data[cellIx + 1], this->data[cellIx + 2]);
  return row;
}

void Matrix2::SetRow(int rowIx, Matrix1 source) {
  int cellIx = rowIx * this->nCols;
  for (int ix = 0; ix < source.Size(); ix++)
    this->data[cellIx + ix] = source(ix);
}

void Matrix2::SetRow3(int rowIx, Vector3 v) {
  int cellIx = rowIx * this->nCols;
  this->data[cellIx + 0] = v.x;
  this->data[cellIx + 1] = v.y;
  this->data[cellIx + 2] = v.z;
}

Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) const {
  Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart);

  for (int i = rowStart; i < rowStop; i++) {
    for (int j = colStart; j < colStop; j++) {
      int resultRowIx = i - rowStart;
      int resultColIx = j - colStart;
      r.data[resultRowIx * r.nCols + resultColIx] = this->data[i * this->nCols + j];
    }
  }
  return r;
}

void Matrix2::UpdateSlice(int rowStart, int rowStop, const Matrix2& m) const {
  UpdateSlice(rowStart, rowStop, 0, this->nCols, m);
}

void Matrix2::UpdateSlice(int rowStart,
                          int rowStop,
                          int colStart,
                          int colStop,
                          const Matrix2& m) const {
  int mRowDataIx = 0;
  // printf("UpdateSlice ====== (%d, %d) -> (%d, %d)\n", rowStart, rowStop,
  //        colStart, colStop);
  for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
    int rRowDataIx = rowIx * this->nCols;
    for (int colIx = colStart; colIx < colStop; colIx++) {
      this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
      // printf("UpdateSlice (%d, %d) -> (%d, %d) = %F\n", mRowDataIx / m.nCols,
      //        (colIx - colStart), rowIx, colIx, m.data[mRowDataIx + (colIx - colStart)]);
    }
    mRowDataIx += m.nCols;
  }
}


// Matrix2 Matrix2::Inverse() {
//   int n = this->nRows;
//   // // Create an identity matrix of the same size as the original matrix
//   Matrix2 augmentedMatrix(n, 2 * n);
//   for (int i = 0; i < n; i++) {
//     for (int j = 0; j < n; j++) {
//       int ix = i * this->nCols + j;
//       augmentedMatrix.data[ix] = this->data[ix];
//       augmentedMatrix.data[ix + n] = (i == j) ? 1 : 0;  // identity matrix;
//     }
//     if (n == 3) {
//       int ix2 = i * this->nCols;
//       std::cerr << augmentedMatrix.data[ix2] << " " << augmentedMatrix.data[ix2+1] << " "
//                 << augmentedMatrix.data[ix2+2] << " " << augmentedMatrix.data[ix2+3] << " "
//                 << augmentedMatrix.data[ix2+4] << " " << augmentedMatrix.data[ix2+5] << "\n";
//     }
//   }

//   // Perform Gaussian elimination
//   for (int i = 0; i < n; i++) {
//     // Find the pivot row
//     float pivot = augmentedMatrix.data[i * augmentedMatrix.nCols + i];
//     std::cerr << "pivot " << (int) i << " | " << (i * augmentedMatrix.nCols + i) << " = " << pivot
// << std::endl; if (abs(pivot) < 1e-10)  // Check for singular matrix std::cerr << "Matrix is singular
// and cannot be inverted. " << (int)i << std::endl;

//     // Normalize the pivot row
//     for (int j = 0; j < 2 * n; j++)
//       augmentedMatrix.data[i * augmentedMatrix.nCols + j] /= pivot;

//     // Eliminate the column below the pivot
//     for (int j = i + 1; j < n; j++) {
//       float factor = augmentedMatrix.data[j * augmentedMatrix.nCols + i];
//       for (int k = 0; k < 2 * n; k++)
//         augmentedMatrix.data[j * augmentedMatrix.nCols + k] -=
//             factor * augmentedMatrix.data[i * augmentedMatrix.nCols + k];
//     }
//   }

//   // Back substitution
//   for (int i = n - 1; i >= 0; i--) {
//     // Eliminate the column above the pivot
//     for (int j = i - 1; j >= 0; j--) {
//       float factor = augmentedMatrix.data[j * augmentedMatrix.nCols + i];
//       for (int k = 0; k < 2 * n; k++)
//         augmentedMatrix.data[j * augmentedMatrix.nCols + k] -=
//             factor * augmentedMatrix.data[i * augmentedMatrix.nCols + k];
//     }
//   }

//   // Extract the inverse matrix from the augmented matrix
//   Matrix2 inverse(n, n);
//   for (int i = 0; i < n; i++) {
//     for (int j = 0; j < n; j++)
//       inverse.data[i * inverse.nCols + j] = augmentedMatrix.data[i * augmentedMatrix.nCols + j + n];
//   }
//   return inverse;
// }

Matrix2 Matrix2::Inverse() {
  int n = this->nRows;              // need to check for nRows = nCols...
  Matrix2 matrix = Matrix2(*this);  // clone this matrix
  Matrix2 inverse = Matrix2::Identity(n);

  // Forward elimination
  for (int i = 0; i < n; ++i) {
    float pivot = matrix(i, i);
    if (abs(pivot) < 1e-10)  // Check for singular matrix
      std::cerr << "Matrix is singular and cannot be inverted. " << (int)i << std::endl;

    for (int j = 0; j < n; ++j) {
      matrix(i, j) /= pivot;   //  Normalize pivot row
      inverse(i, j) /= pivot;  // Apply the same to inverse
    }

    for (int j = 0; j < n; ++j) {
      if (j != i) {  // outside diagonal
        float factor = matrix(j, i);
        for (int k = 0; k < n; ++k) {
          matrix(j, k) -= factor * matrix(i, k);    // Eliminate column
          inverse(j, k) -= factor * inverse(i, k);  // Update inverse
        }
      }
    }
  }
  return inverse;
}

Matrix2 Matrix2::DeleteRows(int rowStart, int rowStop) const {
  Matrix2 r = Matrix2(this->nRows - (rowStop - rowStart), this->nCols);

  int resultRowIx = 0;
  for (int i = 0; i < this->nRows; i++) {
    if (i >= rowStart && i < rowStop)
      continue;

    for (int j = 0; j < this->nCols; j++)
      r.data[resultRowIx * r.nCols + j] = this->data[i * this->nCols + j];

    resultRowIx++;
  }
  return r;
}

Matrix2 Matrix2::DeleteColumns(int colStart, int colStop) const {
  Matrix2 r = Matrix2(this->nRows, this->nCols - (colStop - colStart));

  for (int i = 0; i < this->nRows; i++) {
    int resultColIx = 0;
    for (int j = 0; j < this->nCols; j++) {
      if (j >= colStart && j < colStop)
        continue;

      r.data[i * r.nCols + resultColIx++] = this->data[i * this->nCols + j];
    }
  }
  return r;
}

/// @brief Compute the Omega matrix of a 3D vector
/// @param v The vector
/// @return 4x4 Omega matrix
template <typename T>
Matrix2 Matrix2::Omega(const Vector3Of<T>& v) {
  Matrix2 r = Matrix2::Zero(4, 4);
  r.UpdateSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v));

  // set last row to -v
  int ix = 3 * 4;
  r.data[ix++] = -v.horizontal;
  r.data[ix++] = -v.vertical;
  r.data[ix] = -v.depth;

  // Set last column to v
  ix = 3;
  r.data[ix += 4] = v.horizontal;
  r.data[ix += 4] = v.vertical;
  r.data[ix] = v.depth;
  return r;
}

// Matrix2
#pragma endregion

#pragma region Block // Submatrix2

SubMatrix2::SubMatrix2(const Matrix2& m, int startRow, int startCol, int rowCount, int colCount)
    : parent(m), startRow(startRow), startCol(startCol), rowCount(rowCount), colCount(colCount) {
  assert(this->startRow >= 0 && this->startCol >= 0);
  assert(this->startRow + this->rowCount <= this->parent.NRows() &&
         this->startCol + this->colCount <= this->parent.NCols());
}

void SubMatrix2::operator=(const Matrix2& src) {
  assert(src.NRows() == this->rowCount && src.NCols() == this->colCount);

  // If the block is contiguous in memory and src is contiguous, we can memcpy
  // row by row. In row-major layout, each row of the block is contiguous.
  for (int row = 0; row < this->rowCount; row++) {
    float* dstRowPtr =
        this->parent.GetData() + (this->startRow + row) * this->parent.NCols() + this->startCol;
    const float* srcRowPtr = src.GetData() + row * src.NCols();
    // printf("(%d, %d) %d   (%d -> %d)\n", this->startRow + row, startCol,
    // this->colCount, (int)srcRowPtr, (int)dstRowPtr); memcpy each row
    std::memcpy(dstRowPtr, srcRowPtr, sizeof(float) * this->colCount);
  }
}

void SubMatrix2::operator=(const SubMatrix2& src) {
  assert(src.rowCount == this->rowCount && src.colCount == this->colCount);

  //  If src.parent == parent and regions overlap, we must copy safely (use
  //  temp row buffer).
  bool alias =
      (&src.parent == &this->parent) && !((src.startRow + src.rowCount <= this->startRow) ||
                                          (this->startRow + this->rowCount <= src.startRow) ||
                                          (src.startCol + src.colCount <= this->startCol) ||
                                          (this->startCol + this->colCount <= src.startCol));

  if (!alias) {
    // safe to copy row-by-row directly
    for (int row = 0; row < this->rowCount; row++) {
      float* dstRowPtr =
          this->parent.GetData() + (this->startRow + row) * this->parent.NCols() + this->startCol;
      const float* srcRowPtr =
          src.parent.GetData() + (src.startRow + row) * src.parent.NCols() + src.startCol;
      std::memcpy(dstRowPtr, srcRowPtr, sizeof(float) * this->colCount);
    }
  } else {
    // overlapping regions in same parent: copy to temporary then assign
    Matrix2 tmp(this->rowCount, this->colCount);
    for (int row = 0; row < this->rowCount; row++) {
      const float* srcRowPtr =
          src.parent.GetData() + (src.startRow + row) * src.parent.NCols() + src.startCol;
      std::memcpy(tmp.GetData() + row * this->colCount, srcRowPtr, sizeof(float) * this->colCount);
    }
    // now copy tmp into destination
    for (int row = 0; row < this->rowCount; row++) {
      float* dstRowPtr =
          parent.GetData() + (this->startRow + row) * this->parent.NCols() + this->startCol;
      std::memcpy(dstRowPtr, tmp.GetData() + row * this->colCount, sizeof(float) * this->colCount);
    }
  }
}

float SubMatrix2::operator()(int row, int col) const {
  assert(row >= 0 && col >= 0 && row < rowCount && col < colCount);
  return parent(startRow + row, startCol + col);
}

const float& SubMatrix2::operator()(int row, int col) {
  assert(row >= 0 && col >= 0 && row < rowCount && col < colCount);
  return parent(startRow + row, startCol + col);
}

#pragma endregion Block
*/

#pragma region Matrix2Of

template <typename T>
Matrix2Of<T>::Matrix2Of() {}

template <typename T>
Matrix2Of<T>::Matrix2Of(unsigned int rowCount, unsigned int colCount)
    : nRows(rowCount), nCols(colCount), externalData(false) {
  this->size = this->nRows * this->nCols;

  if (this->size <= 0) {
    this->data = nullptr;
    return;
  }

  this->data = new T[this->size];
}

template <typename T>
Matrix2Of<T>::Matrix2Of(unsigned int rowCount, unsigned int colCount, T* data)
    : nRows(rowCount), nCols(colCount), data(data), externalData(true) {
  this->size = this->nRows * this->nCols;
}

template <typename T>
Matrix2Of<T>::Matrix2Of(const Matrix2Of& m) : nRows(m.nRows), nCols(m.nCols), size(m.size) {
  if (this->size == 0)
    this->data = nullptr;
  else {
    this->data = new T[this->size];

    for (unsigned int ix = 0; ix < this->size; ++ix)
      this->data[ix] = m.data[ix];
  }
}

template <typename T>
Matrix2Of<T>::Matrix2Of(Matrix2Of&& m) noexcept
    : nRows(m.nRows), nCols(m.nCols), size(m.size), data(m.data) {
  m.nRows = 0;
  m.nCols = 0;
  m.size = 0;
  m.data = nullptr;  // Leave the moved-from object in a safe state
}

template <typename T>
Matrix2Of<T>::~Matrix2Of() {
  if (!this->externalData) {
    if (this->size > 0)
      delete[] data;
  }
}

template <typename T>
Matrix2Of<T>& Matrix2Of<T>::operator=(const Matrix2Of& m) {
  if (this != &m) {
    if (this->size > 0) {
      delete[] this->data;  // Free the current memory
    }

    this->nRows = m.nRows;
    this->nCols = m.nCols;
    this->size = m.size;
    this->externalData = m.externalData;
    if (this->size == 0)
      this->data = nullptr;
    else {
      this->data = new T[this->size];
      for (unsigned int ix = 0; ix < this->size; ++ix)
        this->data[ix] = m.data[ix];
    }
  }
  return *this;
}

template <typename T>
Matrix2Of<T>& Matrix2Of<T>::operator=(Matrix2Of&& m) noexcept {
  if (this != &m) {
    if (this->size > 0) {
      delete[] this->data;  // Free the current memory
    }

    // Transfer ownership of resources
    this->nRows = m.nRows;
    this->nCols = m.nCols;
    this->size = m.size;
    this->externalData = m.externalData;
    this->data = m.data;

    // Leave the moved-from object in a valid state
    m.size = 0;
    m.data = nullptr;
  }
  return *this;
}

template <typename T>
unsigned int Matrix2Of<T>::RowCount() const {
  return this->nRows;
}
template <typename T>
unsigned int Matrix2Of<T>::ColCount() const {
  return this->nCols;
}

template <typename T>
const T& Matrix2Of<T>::operator()(unsigned int rowIx, unsigned int colIx) const {
  return data[rowIx * this->nCols + colIx];
}
template <typename T>
T& Matrix2Of<T>::operator()(unsigned int rowIx, unsigned int colIx) {
  return data[rowIx * this->nCols + colIx];
}

template <typename T>
T* Matrix2Of<T>::GetData() {
  return this->data;
}
template <typename T>
T* Matrix2Of<T>::GetData() const {
  return this->data;
}

template <typename T>
Matrix2Of<T> Matrix2Of<T>::Zero(unsigned int rowCount, unsigned int colCount) {
  Matrix2Of r(rowCount, colCount);
  for (unsigned int ix = 0; ix < r.size; ix++)
    r.data[ix] = 0;
  return r;
}

template <typename T>
void Matrix2Of<T>::Fill(T value) {
  for (unsigned int ix = 0; ix < this->size; ix++)
    this->data[ix] = value;
}

template <typename T>
void Matrix2Of<T>::FillDiagonal(T f) {
  int n = this->nRows < this->nCols ? nRows : nCols;
  for (int ix = 0; ix < n; ix++)
    this->data[ix * this->nCols + ix] = f;
}

template <typename T>
Matrix2Of<T> Matrix2Of<T>::Identity(unsigned int size) {
  return Diagonal(1, size);
}

template <typename T>
Matrix2Of<T> Matrix2Of<T>::Identity(unsigned int rowCount, unsigned int colCount) {
  Matrix2Of<T> m = Matrix2Of::Zero(rowCount, colCount);
  m.FillDiagonal(1);
  return m;
}

template <typename T>
Matrix2Of<T> Matrix2Of<T>::Diagonal(T f, unsigned int size) {
  Matrix2Of r = Matrix2Of::Zero(size, size);
  T* data = r.data;
  int valueIx = 0;
  for (unsigned int ix = 0; ix < size; ix++) {
    data[valueIx] = f;
    valueIx += size + 1;
  }
  return r;
}

template <typename T>
Matrix2Of<T> Matrix2Of<T>::SkewMatrix(const Vector3Of<T>& v) {
  Matrix2Of r(3, 3);
  T* data = r.data;
  data[0 * 3 + 0] = 0;
  data[0 * 3 + 1] = -v.depth;    // result(0, 1)
  data[0 * 3 + 2] = v.vertical;  // result(0, 2)
  data[1 * 3 + 0] = v.depth;     // result(1, 0)
  data[1 * 3 + 1] = 0;
  data[1 * 3 + 2] = -v.horizontal;  // result(1, 2)
  data[2 * 3 + 0] = -v.vertical;    // result(2, 0)
  data[2 * 3 + 1] = v.horizontal;   // result(2, 1)
  data[2 * 3 + 2] = 0;
  return r;
}

template <typename T>
Matrix1Of<T> Matrix2Of<T>::GetRow(unsigned int rowIx) {
  Matrix1Of<T> r = Matrix1Of<T>(this->nCols);
  unsigned int rowStartIx = rowIx * this->nCols;
  for (unsigned int colIx = 0; colIx < this->nCols; colIx++) 
    r.data[colIx] = this->data[rowStartIx + colIx];
  return r;
}

template <typename T>
Vector3Of<T> Matrix2Of<T>::GetRow3(unsigned int rowIx) {
  int cellIx = rowIx * this->nCols;
  Vector3Of<T> row(this->data[cellIx + 0], this->data[cellIx + 1], this->data[cellIx + 2]);
  return row;
}

template <typename T>
Matrix2Of<T> Matrix2Of<T>::GetRows(unsigned int fromRowIx, unsigned int toRowIx) {
  if (toRowIx >= this->nRows)
    std::cerr << "Slice index out of range." << std::endl;

  Matrix2Of result = Matrix2Of(toRowIx - fromRowIx, this->nCols);
  int resultRowIx = 0;
  for (unsigned int rowIx = fromRowIx; rowIx < toRowIx; rowIx++) {
    for (unsigned int colIx = 0; colIx < this->nCols; colIx++)
      result.data[resultRowIx * result.nCols + colIx] = this->data[rowIx * this->nCols + colIx];

    resultRowIx++;
  }

  return result;
}

template <typename T>
void Matrix2Of<T>::SetRow(unsigned int rowIx, Matrix1Of<T> source) {
  int cellIx = rowIx * this->nCols;
  for (unsigned int ix = 0; ix < source.Size(); ix++)
    this->data[cellIx + ix] = source(ix);
}

template <typename T>
void Matrix2Of<T>::SetRow3(unsigned int rowIx, Vector3Of<T> v) {
  int cellIx = rowIx * this->nCols;
  this->data[cellIx + 0] = v.horizontal;
  this->data[cellIx + 1] = v.vertical;
  this->data[cellIx + 2] = v.depth;
}

template <typename T>
Matrix2Of<T> Matrix2Of<T>::DeleteRows(unsigned int fromRowIx, unsigned int toRowIx) const {
  Matrix2Of r(this->nRows - (toRowIx - fromRowIx), this->nCols);

  int resultRowIx = 0;
  for (unsigned int i = 0; i < this->nRows; i++) {
    if (i >= fromRowIx && i < toRowIx)
      continue;

    for (unsigned int j = 0; j < this->nCols; j++)
      r.data[resultRowIx * r.nCols + j] = this->data[i * this->nCols + j];

    resultRowIx++;
  }
  return r;
}

template <typename T>
Matrix2Of<T> Matrix2Of<T>::DeleteColumns(unsigned int fromColIx, unsigned int toColIx) const {
  Matrix2Of r(this->nRows, this->nCols - (toColIx - fromColIx));

  for (unsigned int i = 0; i < this->nRows; i++) {
    int resultColIx = 0;
    for (unsigned int j = 0; j < this->nCols; j++) {
      if (j >= fromColIx && j < toColIx)
        continue;

      r.data[i * r.nCols + resultColIx++] = this->data[i * this->nCols + j];
    }
  }
  return r;
}

template <typename T>
Matrix2Of<T> Matrix2Of<T>::operator-() const {
  Matrix2Of r(this->nRows, this->nCols);
  for (unsigned int ix = 0; ix < r.size; ix++)
    r.data[ix] = -this->data[ix];
  return r;
}

template <typename T>
Matrix2Of<T> Matrix2Of<T>::operator+(const Matrix2Of& v) const {
  Matrix2Of r(this->nRows, this->nCols);
  for (unsigned int ix = 0; ix < r.size; ix++)
    r.data[ix] = this->data[ix] + v.data[ix];
  return r;
}

template <typename T>
Matrix2Of<T>& Matrix2Of<T>::operator+=(const Matrix2Of& v) {
  for (unsigned int ix = 0; ix < this->size; ix++)
    this->data[ix] += v.data[ix];
  return *this;
}

template <typename T>
Matrix2Of<T> Matrix2Of<T>::operator-(const Matrix2Of& v) const {
  Matrix2Of r(this->nRows, this->nCols);
  for (unsigned int ix = 0; ix < r.size; ix++)
    r.data[ix] = this->data[ix] - v.data[ix];
  return r;
}

template <typename T>
Matrix2Of<T>& Matrix2Of<T>::operator-=(const Matrix2Of& v) {
  for (unsigned int ix = 0; ix < this->size; ix++)
    this->data[ix] -= v.data[ix];
  return *this;
}

template <typename T>
Matrix2Of<T> Matrix2Of<T>::operator*(const Matrix2Of& B) const {
  Matrix2Of r(this->nRows, B.nCols);

  const int ACols = this->nCols;
  const int BCols = B.nCols;
  const int ARows = this->nRows;

  for (int i = 0; i < ARows; ++i) {
    // Pre-compute row offsets
    int ARowOffset = i * ACols;  // ARowOffset is constant for each row of A
    int BColOffset = i * BCols;  // BColOffset is constant for each row of B
    for (int j = 0; j < BCols; ++j) {
      T sum = 0;
      int BIndex = j;
      for (int k = 0; k < ACols; ++k) {
        sum += this->data[ARowOffset + k] * B.data[BIndex];
        BIndex += BCols;
      }
      r.data[BColOffset + j] = sum;

    }
  }

  return r;
}


template <typename T>
Matrix2Of<T> Matrix2Of<T>::Transposed() const {
  Matrix2Of r(this->nCols, this->nRows);

  for (unsigned int rowIx = 0; rowIx < this->nRows; rowIx++) {
    for (unsigned int colIx = 0; colIx < this->nCols; colIx++)
      r.data[colIx * this->nRows + rowIx] = this->data[rowIx * this->nCols + colIx];
  }
  return r;
}

template <typename T>
Matrix2Of<T> Matrix2Of<T>::Inverse() {
  int n = this->nRows;              // need to check for nRows = nCols...
  Matrix2Of matrix = Matrix2Of(*this);  // clone this matrix
  Matrix2Of inverse = Matrix2Of::Identity(n);

  // Forward elimination
  for (int i = 0; i < n; ++i) {
    T pivot = matrix(i, i);
    if (abs(pivot) < 1e-10)  // Check for singular matrix
      std::cerr << "Matrix is singular and cannot be inverted. " << (int)i << std::endl;

    for (int j = 0; j < n; ++j) {
      matrix(i, j) /= pivot;   //  Normalize pivot row
      inverse(i, j) /= pivot;  // Apply the same to inverse
    }

    for (int j = 0; j < n; ++j) {
      if (j != i) {  // outside diagonal
        T factor = matrix(j, i);
        for (int k = 0; k < n; ++k) {
          matrix(j, k) -= factor * matrix(i, k);    // Eliminate column
          inverse(j, k) -= factor * inverse(i, k);  // Update inverse
        }
      }
    }
  }
  return inverse;
}

template <typename T>
Matrix2Of<T> Matrix2Of<T>::Slice(int rowStart, int rowStop, int colStart, int colStop) const {
  Matrix2Of r(rowStop - rowStart, colStop - colStart);

  for (int i = rowStart; i < rowStop; i++) {
    for (int j = colStart; j < colStop; j++) {
      int resultRowIx = i - rowStart;
      int resultColIx = j - colStart;
      r.data[resultRowIx * r.nCols + resultColIx] = this->data[i * this->nCols + j];
    }
  }
  return r;
}

template <typename T>
void Matrix2Of<T>::UpdateSlice(int rowStart, int rowStop, const Matrix2Of& m) const {
  UpdateSlice(rowStart, rowStop, 0, this->nCols, m);
}

template <typename T>
void Matrix2Of<T>::UpdateSlice(int rowStart,
                          int rowStop,
                          int colStart,
                          int colStop,
                          const Matrix2Of& m) const {
  int mRowDataIx = 0;
  // printf("UpdateSlice ====== (%d, %d) -> (%d, %d)\n", rowStart, rowStop,
  //        colStart, colStop);
  for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
    int rRowDataIx = rowIx * this->nCols;
    for (int colIx = colStart; colIx < colStop; colIx++) {
      this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
      // printf("UpdateSlice (%d, %d) -> (%d, %d) = %F\n", mRowDataIx / m.nCols,
      //        (colIx - colStart), rowIx, colIx, m.data[mRowDataIx + (colIx - colStart)]);
    }
    mRowDataIx += m.nCols;
  }
}

// template <typename T>
// Matrix2Of<T> Matrix2Of<T>::Omega(const Vector3Of<T>& v) {
//   Matrix2Of r = Matrix2Of::Zero(4, 4);
//   r.UpdateSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v));

//   // set last row to -v
//   int ix = 3 * 4;
//   r.data[ix++] = -v.horizontal;
//   r.data[ix++] = -v.vertical;
//   r.data[ix] = -v.depth;

//   // Set last column to v
//   ix = 3;
//   r.data[ix += 4] = v.horizontal;
//   r.data[ix += 4] = v.vertical;
//   r.data[ix] = v.depth;
//   return r;
// }

template class Matrix2Of<int>;
template class Matrix2Of<float>;
template class Matrix2Of<double>;

#pragma endregion Matrix2Of

}  // namespace LinearAlgebra