From 2eaea20d3aaf9d353cf5fcd2ea683b8485684bb4 Mon Sep 17 00:00:00 2001
From: Jaby <Jaby>
Date: Thu, 1 Feb 2024 21:29:13 -0500
Subject: [PATCH] Implement Matrix stack

---
 include/PSX/GTE/gte.hpp                   |  7 ++--
 src/Library/src/BootLoader/start_boot.cpp | 44 ++++++++++-------------
 src/Library/src/GTE/gte.cpp               | 18 ++++++++--
 3 files changed, 38 insertions(+), 31 deletions(-)

diff --git a/include/PSX/GTE/gte.hpp b/include/PSX/GTE/gte.hpp
index 0b434bd3..dae1c0a7 100644
--- a/include/PSX/GTE/gte.hpp
+++ b/include/PSX/GTE/gte.hpp
@@ -3,6 +3,8 @@
 
 namespace JabyEngine {
     namespace GTE {
+        static constexpr auto StackSize = 16;
+
         /*
             RotTrans
 
@@ -123,12 +125,13 @@ namespace JabyEngine {
             Optional: replaces current matrix (rotation and parallel) with input
         */
         void push_matrix();
-        void push_matrix(const MATRIX& matrix);
+        void push_matrix_and_set(const MATRIX& matrix);
 
         /*
             Restores the previous stored matrix (rotation and parallel)
         */
-        void pop_matrix();
+        MATRIX get_and_pop_matrix();
+        void   pop_matrix();
 
         /*
             SetGeomOffset(ofx,ofy)
diff --git a/src/Library/src/BootLoader/start_boot.cpp b/src/Library/src/BootLoader/start_boot.cpp
index 802f8fd3..0ea5a5cf 100644
--- a/src/Library/src/BootLoader/start_boot.cpp
+++ b/src/Library/src/BootLoader/start_boot.cpp
@@ -111,38 +111,30 @@ namespace JabyEngine {
             printf("{%i, %i, %i}\n", matrix.trans[0],  matrix.trans[1],  matrix.trans[2]);
         };
 
+        static const auto make_matrix = [](int16_t mult) -> GTE::MATRIX {
+            return GTE::MATRIX {
+                .rot   = {
+                    {static_cast<int16_t>(1*mult), static_cast<int16_t>(2*mult), static_cast<int16_t>(3*mult)},
+                    {static_cast<int16_t>(2*mult), static_cast<int16_t>(3*mult), static_cast<int16_t>(1*mult)},
+                    {static_cast<int16_t>(3*mult), static_cast<int16_t>(1*mult), static_cast<int16_t>(2*mult)},
+                },
+                .trans = {6*mult, 7*mult, 8*mult}
+            };
+        };
+
         display_stuff(25);
         display_stuff(45);
         display_stuff(75);
         display_stuff(90);
 
-        auto rot_mat = []() -> GTE::MATRIX {
-            auto rot = GTE::MATRIX::identity();
+        for(size_t n = 0; n < 3; n++) {
+            GTE::push_matrix_and_set(make_matrix(n + 1));
+        }
 
-            rot.rot[0][0] = 123;
-            rot.rot[0][1] = 456;
-            rot.rot[0][2] = 789;
-
-            rot.rot[1][0] = 1123;
-            rot.rot[1][1] = 1456;
-            rot.rot[1][2] = 1789;
-
-            rot.rot[2][0] = 2123;
-            rot.rot[2][1] = 2456;
-            rot.rot[2][2] = 2789;
-
-            rot.trans[0] = 9;
-            rot.trans[1] = 8;
-            rot.trans[2] = 7;
-            return rot;
-        }();
-
-        print_matrix(rot_mat);
-        printf("---\n");
-        GTE::set_matrix(rot_mat);
-        rot_mat = GTE::get_matrix();
-        print_matrix(rot_mat);
-        printf("---\n");
+        for(size_t n = 0; n < 3; n++) {
+            print_matrix(GTE::get_and_pop_matrix());
+            printf("---\n");
+        }
     }
 
     void start() {
diff --git a/src/Library/src/GTE/gte.cpp b/src/Library/src/GTE/gte.cpp
index 44aebada..5a711ea3 100644
--- a/src/Library/src/GTE/gte.cpp
+++ b/src/Library/src/GTE/gte.cpp
@@ -26,6 +26,9 @@ int32_t cos(int32_t value) {
 
 namespace JabyEngine {
     namespace GTE {
+        static MATRIX  Stack[StackSize];
+        static MATRIX* FreeStackEntry = Stack;
+
         MATRIX& multiply_matrix(const MATRIX& m0, const MATRIX& m1, MATRIX& result) {
             set_rot_matrix(m0);
             
@@ -58,16 +61,25 @@ namespace JabyEngine {
         }
 
         void push_matrix() {
-            // FIXME: Implement this
+            *FreeStackEntry = get_matrix();
+            FreeStackEntry++;
         }
 
-        void push_matrix(const MATRIX& matrix) {
+        void push_matrix_and_set(const MATRIX& matrix) {
             push_matrix();
             set_matrix(matrix);
         }
 
         void pop_matrix() {
-            // FIXME: Implement this
+            FreeStackEntry--;
+            set_matrix(*FreeStackEntry);
         }
+
+        MATRIX get_and_pop_matrix() {
+            const auto matrix = get_matrix();
+            pop_matrix();
+
+            return matrix;
+        }        
     }
 }
\ No newline at end of file