RoboidControl-cpp/LinearAlgebra/Matrix.cpp

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

#include "esp_log.h"
#include "esp_system.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

namespace LinearAlgebra {

#pragma region Matrix1

Matrix1::Matrix1() {}

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

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

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

Matrix1 Matrix1::FromVector3(Vector3 v) {
  Matrix1 r(3);
  r.data[0] = v.x;
  r.data[1] = v.y;
  r.data[2] = v.z;
  return r;
}

Vector3 Matrix1::ToVector3() {
  return Vector3(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& m) const {
  for (int ix = start; ix < stop; ix++) {
    this->data[ix] = m.data[ix - start];
  }
}

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

// Matrix1
#pragma endregion

#pragma region Matrix2

// Function to check available heap memory
size_t getAvailableHeapMemory() {
  // This is a simple example; you may need to implement a more robust method
  // depending on your platform. For ESP32, you can use heap_4.c or similar.
  return esp_get_free_heap_size();  // Example for ESP32
}

Matrix2::Matrix2() {}

Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
  this->nValues = nRows * nCols;
  // printf("Allocate %d\n", this->nValues);

  if (this->nValues <= 0) {
    this->data = nullptr;
    this->externalData = false;
    return;
  }
  // Check available heap memory before allocation
  size_t availableMemory = getAvailableHeapMemory();
  if (availableMemory < this->nValues * sizeof(float)) {
    std::cerr << "Error: Not enough memory to allocate Matrix2 of size "
              << this->nValues << " floats." << std::endl;
    this->data = nullptr;  // Handle memory allocation failure
    this->externalData = false;
    return;
  }

  UBaseType_t stack_size =
      uxTaskGetStackHighWaterMark(NULL);  // NULL to check the main task
  if (stack_size < 1000)
    std::cerr << "Stack High Water Mark: " << stack_size << std::endl;

  try {
    this->data = new float[this->nValues];
    this->externalData = false;
  } catch (const std::bad_alloc& e) {
    std::cerr << "Memory allocation failed: " << e.what() << std::endl;
    this->data = nullptr;  // Handle memory allocation failure
    this->externalData = false;
  }
}

Matrix2::Matrix2(int nRows, int nCols, float* data)
    : nRows(nRows), nCols(nCols), data(data) {
  this->nValues = nRows * nCols;
  this->externalData = true;
}

Matrix2::Matrix2(const Matrix2& m)
    : nRows(m.nRows), nCols(m.nCols), nValues(m.nValues) {
  if (this->nValues == 0)
    this->data = nullptr;
  else {
    this->data = new float[this->nValues];

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

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

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

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

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

// Move constructor
// Matrix2::Matrix2(Matrix2&& other) noexcept
//     : nRows(other.nRows),
//       nCols(other.nCols),
//       nValues(other.nValues),
//       data(other.data) {
//   other.data = nullptr;  // Set the other object's pointer to nullptr to
//   avoid
//                          // double deletion
// }

// Move assignment operator
// Matrix2& Matrix2::operator=(Matrix2&& other) noexcept {
//   if (this != &other) {
//     delete[] data;  // Clean up current data
//     nRows = other.nRows;
//     nCols = other.nCols;
//     nValues = other.nValues;
//     data = other.data;
//     other.data = nullptr;  // Avoid double deletion
//   }
//   return *this;
// }

Matrix2 Matrix2::Zero(int nRows, int nCols) {
  Matrix2 r = Matrix2(nRows, nCols);
  for (int ix = 0; ix < r.nValues; 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::Clear() {
  for (int ix = 0; ix < this->nValues; ix++)
    this->data[ix] = 0;
}

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

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 Vector3& v) {
  Matrix2 r = Matrix2(3, 3);
  float* data = r.data;
  data[0 * 3 + 1] = -v.z;  // result(0, 1)
  data[0 * 3 + 2] = v.y;   // result(0, 2)
  data[1 * 3 + 0] = v.z;   // result(1, 0)
  data[1 * 3 + 2] = -v.x;  // result(1, 2)
  data[2 * 3 + 0] = -v.y;  // result(2, 0)
  data[2 * 3 + 1] = v.x;   // result(2, 1)
  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->nCols + 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.nValues; 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.nValues; ix++)
    r.data[ix] = this->data[ix] + v.data[ix];
  return r;
}

Matrix2& Matrix2::operator+=(const Matrix2& v) {
  for (int ix = 0; ix < this->nValues; 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.nValues; 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(rowIx);
  }
  return r;
}

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

  int ACols = this->nCols;
  int BCols = B.nCols;
  int ARows = this->nRows;
  // 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;
      std::cout << " 0";
      int BIndex = j;
      for (int k = 0; k < ACols; ++k) {
        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";
    }
  }
  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) {
  Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart);

  int resultRowIx = 0;
  int resultColIx = 0;
  for (int i = rowStart; i < rowStop; i++) {
    for (int j = colStart; j < colStop; j++)
      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;
  for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
    int rRowDataIx = rowIx * this->nCols;
    mRowDataIx += m.nCols;
    for (int colIx = colStart; colIx < colStop; colIx++) {
      this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
    }
  }
}

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++) {
      augmentedMatrix.data[i * this->nCols + j] =
          this->data[i * this->nCols + j];
      augmentedMatrix.data[i * this->nCols + (j + n)] =
          (i == j) ? 1 : 0;  // identity matrix;
    }
  }

  // Perform Gaussian elimination
  for (int i = 0; i < n; i++) {
    // Find the pivot row
    float pivot = augmentedMatrix.data[i * augmentedMatrix.nCols + i];
    if (abs(pivot) < 1e-10)  // Check for singular matrix
      std::cerr << "Matrix is singular and cannot be inverted." << 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::DeleteRows(int rowStart, int rowStop) {
  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) {
  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
Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& 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.x;
  r.data[ix++] = -v.y;
  r.data[ix] = -v.z;

  // Set last column to v
  ix = 3;
  r.data[ix += 4] = v.x;
  r.data[ix += 4] = v.y;
  r.data[ix] = v.z;
  return r;
}

// Matrix2
#pragma endregion

}  // namespace LinearAlgebra

template <>
MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
  if (rows <= 0 || cols <= 0) {
    this->rows = 0;
    this->cols = 0;
    this->data = nullptr;
    return;
  }
  this->rows = rows;
  this->cols = cols;

  unsigned int matrixSize = this->cols * this->rows;
  this->data = new float[matrixSize]{0.0f};
}

template <>
MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
  Set(0, 0, v.Right());
  Set(1, 0, v.Up());
  Set(2, 0, v.Forward());
}

template <>
void MatrixOf<float>::Multiply(const MatrixOf<float>* m1,
                               const MatrixOf<float>* m2,
                               MatrixOf<float>* r) {
  for (unsigned int rowIx1 = 0; rowIx1 < m1->rows; rowIx1++) {
    for (unsigned int colIx2 = 0; colIx2 < m2->cols; colIx2++) {
      unsigned int rDataIx = colIx2 * m2->cols + rowIx1;
      r->data[rDataIx] = 0.0F;
      for (unsigned int kIx = 0; kIx < m2->rows; kIx++) {
        unsigned int dataIx1 = rowIx1 * m1->cols + kIx;
        unsigned int dataIx2 = kIx * m2->cols + colIx2;
        r->data[rDataIx] += m1->data[dataIx1] * m2->data[dataIx2];
      }
    }
  }
}

template <>
Vector3 MatrixOf<float>::Multiply(const MatrixOf<float>* m, Vector3 v) {
  MatrixOf<float> v_m = MatrixOf<float>(v);
  MatrixOf<float> r_m = MatrixOf<float>(3, 1);

  Multiply(m, &v_m, &r_m);

  Vector3 r = Vector3(r_m.data[0], r_m.data[1], r_m.data[2]);
  return r;
}

template <typename T>
Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
  float* vData = new float[3]{v.Right(), v.Up(), v.Forward()};
  MatrixOf<float> v_m = MatrixOf<float>(3, 1, vData);
  float* rData = new float[3]{};
  MatrixOf<float> r_m = MatrixOf<float>(3, 1, rData);

  Multiply(this, &v_m, &r_m);

  Vector3 r = Vector3(r_m.data[0], r_m.data[1], r_m.data[2]);
  delete[] vData;
  delete[] rData;
  return r;
}