From ca5abf097bdef5919c4b105462342e1d2e865378 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pse@pale.blue>
Date: Wed, 2 Jul 2025 17:20:13 +0200
Subject: [PATCH] Running better, but crashing because memory is filling up...

---
 LinearAlgebra/Matrix.cpp | 46 ++++++++++++++++++++++++++++++++++------
 1 file changed, 40 insertions(+), 6 deletions(-)

diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp
index e792a36..d88c0ee 100644
--- a/LinearAlgebra/Matrix.cpp
+++ b/LinearAlgebra/Matrix.cpp
@@ -3,6 +3,11 @@
 #include <iostream>
 #endif
 
+#include "esp_system.h"
+#include "esp_log.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
 namespace LinearAlgebra {
 
 #pragma region Matrix1
@@ -70,15 +75,44 @@ float& Matrix1::operator()(int ix) {
 
 #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;
-  if (this->nValues == 0)
+  if (this->nValues <= 0) {
     this->data = nullptr;
-  else {
+    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;
   }
 }
 
@@ -161,10 +195,10 @@ Matrix2 Matrix2::Zero(int nRows, int nCols) {
 }
 
 const float& Matrix2::operator()(int row, int col) const {
-  return data[row + this->nCols + col];
+  return data[row * this->nCols + col];
 }
 float& Matrix2::operator()(int row, int col) {
-  return data[row + this->nCols + col];
+  return data[row * this->nCols + col];
 }
 
 void Matrix2::Clear() {
@@ -285,8 +319,8 @@ Matrix2 Matrix2::GetRows(int from, int to) {
 
 Vector3 Matrix2::GetRow3(int rowIx) {
   int cellIx = rowIx * this->nCols;
-  Vector3 row(this->data[cellIx, 0], this->data[cellIx, 1],
-              this->data[cellIx, 2]);
+  Vector3 row(this->data[cellIx + 0], this->data[cellIx + 1],
+              this->data[cellIx + 2]);
   return row;
 }