diff --git a/CMakeLists.txt b/CMakeLists.txt
index 83d242ac..31c81b1f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -95,6 +95,7 @@ list(FILTER ALL_FILES EXCLUDE REGEX "src/main/fox_edata_1C.c")
 list(FILTER ALL_FILES EXCLUDE REGEX "src/main/fox_rcp_setup.c")
 list(FILTER ALL_FILES EXCLUDE REGEX "src/main/fox_load_inits.c")
 list(FILTER ALL_FILES EXCLUDE REGEX "src/overlays/ovl_ending/fox_end2_data.c")
+list(FILTER ALL_FILES EXCLUDE REGEX "src/main/sys_timer.c")
 
 # list(FILTER ALL_FILES EXCLUDE REGEX "src/main/fox_hud.c")
 # list(FILTER ALL_FILES EXCLUDE REGEX "src/main/fox_360.c")
diff --git a/include/libc/math.h b/include/libc/math.h
index 1f243d23..c8b4b596 100644
--- a/include/libc/math.h
+++ b/include/libc/math.h
@@ -2,8 +2,9 @@
 #define LIBC_MATH_H
 
 #include <libultra/types.h>
+#include <math.h>
 
-#define M_PI    3.14159265358979323846f
+#define M_PI    3.14159265358979323846264338327950288
 #define M_DTOR	(M_PI / 180.0f)
 #define M_RTOD	(180.0f / M_PI)
 #define M_SQRT2 1.41421356237309504880f
@@ -12,22 +13,6 @@
 #define SHT_MAX 32767.0f
 #define SHT_MINV (1.0f / SHT_MAX)
 
-typedef union {
-    struct {
-        u32 hi;
-        u32 lo;
-    } word;
-
-    f64 d;
-} du;
-
-typedef union {
-    u32 i;
-    f32 f;
-} fu;
-
-extern f32 __libm_qnan_f;
-
 #define __floorf floorf
 #define __floor floor
 #define __lfloorf lfloorf
diff --git a/include/macros.h b/include/macros.h
index 27fa2324..66b3040d 100644
--- a/include/macros.h
+++ b/include/macros.h
@@ -21,7 +21,7 @@
 #define RAND_INT_SEEDED(max) ((s32)(Rand_ZeroOneSeeded()*(max)))
 #define RAND_FLOAT_CENTERED_SEEDED(width) ((Rand_ZeroOneSeeded()-0.5f)*(width))
 
-#define SEGMENTED_TO_VIRTUAL(segment) ((void*)OS_PHYSICAL_TO_K0(gSegments[((uintptr_t)(segment)<<4)>>0x1C]+(((uintptr_t)(segment))&0xFFFFFF)))
+#define SEGMENTED_TO_VIRTUAL(segment) (segment)
 
 #define ARRAY_COUNT(arr) (s32)(sizeof(arr) / sizeof(arr[0]))
 #define ARRAY_COUNTU(arr) (u32)(sizeof(arr) / sizeof(arr[0]))
@@ -39,15 +39,15 @@
 #define RAD_TO_DEG(radians) (((radians) * 180.0f) / M_PI)
 #define DEG_TO_RAD(degrees) (((degrees) / 180.0f) * M_PI)
 
-// TODO: Fix these
-#define SIN_DEG(angle) /*sinf*/(M_DTOR * angle)
-#define COS_DEG(angle) /*cosf*/(M_DTOR * angle)
+extern f32 SIN_DEG(f32 angle);
+extern f32 COS_DEG(f32 angle);
 
 #define USEC_TO_CYCLES(n) (((u64)(n)*(OS_CLOCK_RATE/15625LL))/(1000000LL/15625LL))
 #define MSEC_TO_CYCLES(n) (USEC_TO_CYCLES((n) * 1000LL))
 
 #define CYCLES_TO_USEC(c)    (((u64)(c)*(1000000LL/15625LL))/(OS_CLOCK_RATE/15625LL))
 #define CYCLES_TO_MSEC(c) ((s32)CYCLES_TO_USEC(c)/1000)
+#define CYCLES_TO_MSEC_PC(c) (CYCLES_TO_USEC(c)/1000)
 
 /*
  * Macros for libultra
diff --git a/include/sf64math.h b/include/sf64math.h
index defb41af..839011bd 100644
--- a/include/sf64math.h
+++ b/include/sf64math.h
@@ -49,9 +49,9 @@ extern Mtx gIdentityMtx; // 800C4620
 extern Matrix gIdentityMatrix; //800C4660
 
 extern Matrix* gGfxMatrix;
-extern Matrix sGfxMatrixStack[0x20];
+extern Matrix sGfxMatrixStack[];
 extern Matrix* gCalcMatrix;
-extern Matrix sCalcMatrixStack[0x20];
+extern Matrix sCalcMatrixStack[];
 
 f32 Math_ModF(f32 value, f32 mod);
 void Rand_Init(void);
diff --git a/include/sf64thread.h b/include/sf64thread.h
index 6c97ca0c..31aeb1ce 100644
--- a/include/sf64thread.h
+++ b/include/sf64thread.h
@@ -28,7 +28,7 @@ typedef struct {
     /* 0x00 */ u8 active;
     /* 0x08 */ OSTimer timer;
     /* 0x28 */ TimerAction action;
-    /* 0x2C */ s32* address;
+    /* 0x2C */ uintptr_t address;
     /* 0x30 */ s32 value;
 } TimerTask; // size = 0x38, 0x8 aligned
 
diff --git a/libultraship b/libultraship
index 8cbc5752..f7c80fc8 160000
--- a/libultraship
+++ b/libultraship
@@ -1 +1 @@
-Subproject commit 8cbc575218515c5518319e91870c404b86e87c36
+Subproject commit f7c80fc8f7d77c5a1340afcac02a102731665d33
diff --git a/src/audio/audio_general.c b/src/audio/audio_general.c
index 1e4f5b00..7cd092a6 100644
--- a/src/audio/audio_general.c
+++ b/src/audio/audio_general.c
@@ -1724,6 +1724,9 @@ void Audio_PlayActiveSfx(u8 bankId) {
 }
 
 void Audio_KillSfxByBank(u8 bankId) {
+    // LAudioTODO: Stub for now
+    return;
+
     SfxBankEntry* entry;
     u8 next = sSfxBanks[bankId][0].next;
 
@@ -1744,6 +1747,8 @@ void Audio_KillSfxByBank(u8 bankId) {
 }
 
 void Audio_StopSfxByBankAndSource(u8 bankId, f32* sfxSource) {
+    // LAudioTODO: Stub for now
+    return;
     SfxBankEntry* entry;
     u8 curIndex = 0;
     u8 nextIndex = sSfxBanks[bankId][0].next;
@@ -1765,6 +1770,8 @@ void Audio_StopSfxByBankAndSource(u8 bankId, f32* sfxSource) {
 }
 
 void Audio_KillSfxByBankAndSource(u8 bankId, f32* sfxSource) {
+    // LAudioTODO: Stub for now
+    return;
     SfxBankEntry sp18;
 
     Audio_StopSfxByBankAndSource(bankId, sfxSource);
@@ -1774,6 +1781,8 @@ void Audio_KillSfxByBankAndSource(u8 bankId, f32* sfxSource) {
 }
 
 void Audio_KillSfxBySource(f32* sfxSource) {
+    // LAudioTODO: Stub for now
+    return;
     u8 i;
     SfxBankEntry sp24;
 
@@ -1785,6 +1794,9 @@ void Audio_KillSfxBySource(f32* sfxSource) {
 }
 
 void Audio_KillSfxBySourceAndId(f32* sfxSource, u32 sfxId) {
+    // LAudioTODO: Stub for now
+    return;
+
     u32 bankId = SFX_BANK(sfxId);
     u8 next = sSfxBanks[bankId][0].next;
     u8 current = 0;
@@ -1812,6 +1824,8 @@ void Audio_KillSfxBySourceAndId(f32* sfxSource, u32 sfxId) {
 }
 
 void Audio_KillSfxByTokenAndId(u8 token, u32 sfxId) {
+    // LAudioTODO: Stub for now
+    return;
     u32 bankId = SFX_BANK(sfxId);
     u8 next = sSfxBanks[bankId][0].next;
     u8 current = 0;
@@ -1838,6 +1852,8 @@ void Audio_KillSfxByTokenAndId(u8 token, u32 sfxId) {
 }
 
 void Audio_KillSfxById(u32 sfxId) {
+    // LAudioTODO: Stub for now
+    return;
     u32 bankId = SFX_BANK(sfxId);
     u8 next = sSfxBanks[bankId][0].next;
     u8 current = 0;
@@ -1991,6 +2007,8 @@ s32 Audio_GetCurrentVoice(void) {
 }
 
 s32 Audio_GetCurrentVoiceStatus(void) {
+    // LAudioTODO: Stub for now
+    return 0;
     SequenceChannel* channel = gSeqPlayers[SEQ_PLAYER_VOICE].channels[15];
     SequenceLayer* layer = channel->layers[0];
 
diff --git a/src/engine/fox_load.c b/src/engine/fox_load.c
index 5ad54e8c..350c0fbe 100644
--- a/src/engine/fox_load.c
+++ b/src/engine/fox_load.c
@@ -27,24 +27,28 @@ OverlayInit sCurrentOverlay = {
 };
 
 void Overlay_LoadSegment(void* vRomAddress, void* dest, ptrdiff_t size) {
-    // s32 i;
-    // for (i = 0; gDmaTable[i].pRom.end != NULL; i++) {
-    //     if (gDmaTable[i].vRomAddress == vRomAddress) {
-    //         if (gDmaTable[i].compFlag == 0) {
-    //             Lib_DmaRead(gDmaTable[i].pRom.start, dest, size);
-    //         } else {
-    //             Lib_FillScreen(true);
-    //             sFillTimer = 3;
-    //             D_80161A39 = true;
-    //             Lib_DmaRead(gDmaTable[i].pRom.start, gFrameBuffers, SEGMENT_SIZE(gDmaTable[i].pRom));
-    //             Mio0_Decompress(gFrameBuffers, dest);
-    //         }
-    //         break;
-    //     }
-    // }
+    s32 i;
+    for (i = 0; gDmaTable[i].pRom.end != NULL; i++) {
+        if (gDmaTable[i].vRomAddress == vRomAddress) {
+            if (gDmaTable[i].compFlag == 0) {
+                Lib_DmaRead(gDmaTable[i].pRom.start, dest, size);
+            } else {
+                Lib_FillScreen(true);
+                sFillTimer = 3;
+                D_80161A39 = true;
+                // Lib_DmaRead(gDmaTable[i].pRom.start, gFrameBuffers, SEGMENT_SIZE(gDmaTable[i].pRom));
+                // Mio0_Decompress(gFrameBuffers, dest);
+            }
+            break;
+        }
+    }
 }
 
 u8 Overlay_Init(OverlayInit* ovlInit) {
+
+    sCurrentOverlay = *ovlInit;
+    return true;
+
     u8* ramPtr = SEGMENT_VRAM_START(ovl_i1);
     u8 segment;
     u8 changeOvl = false;
@@ -174,13 +178,13 @@ u8 Overlay_Load(u8 ovlSetup, u8 ovlStage) {
         case OVL_SETUP_VERSUS:
             changeOvl = Overlay_Init(&sOvli2_Versus[ovlStage]);
             if (changeOvl == true) {
-                Audio_SetAudioSpec(3, 0x310);
+                // Audio_SetAudioSpec(3, 0x310);
             }
             break;
         case OVL_SETUP_LOGO:
             changeOvl = Overlay_Init(&sNoOvl_Logo[ovlStage]); // Logo does not load an overlay file
             if (changeOvl == true) {
-                Audio_SetAudioSpec(0, 0xE);
+                // Audio_SetAudioSpec(0, 0xE);
             }
             break;
         case OVL_SETUP_CREDITS:
diff --git a/src/engine/fox_save.c b/src/engine/fox_save.c
index 554ab310..d7925868 100644
--- a/src/engine/fox_save.c
+++ b/src/engine/fox_save.c
@@ -53,6 +53,8 @@ s32 Save_Write(void) {
 }
 
 s32 Save_Read(void) {
+
+    return 0;
     OSMesg* sp24;
     s32 i;
 
diff --git a/src/libultra/gu/mtxutil.c b/src/libultra/gu/mtxutil.c
index e4521bb1..a89c46ec 100644
--- a/src/libultra/gu/mtxutil.c
+++ b/src/libultra/gu/mtxutil.c
@@ -13,54 +13,50 @@
 #include <libultraship.h>
 
 void guMtxF2L(float mf[4][4], Mtx* m) {
-    int i, j;
-    int e1, e2;
-    int *ai, *af;
-
-    ai = (int*) &m->m[0][0];
-    af = (int*) &m->m[2][0];
-
-    for (i = 0; i < 4; i++) {
-        for (j = 0; j < 2; j++) {
-            e1 = FTOFIX32(mf[i][j * 2]);
-            e2 = FTOFIX32(mf[i][j * 2 + 1]);
-            *(ai++) = (e1 & 0xffff0000) | ((e2 >> 16) & 0xffff);
-            *(af++) = ((e1 << 16) & 0xffff0000) | (e2 & 0xffff);
+    unsigned int r, c;
+    s32 tmp1;
+    s32 tmp2;
+    s32* m1 = &m->m[0][0];
+    s32* m2 = &m->m[2][0];
+    for (r = 0; r < 4; r++) {
+        for (c = 0; c < 2; c++) {
+            tmp1 = mf[r][2 * c] * 65536.0f;
+            tmp2 = mf[r][2 * c + 1] * 65536.0f;
+            *m1++ = (tmp1 & 0xffff0000) | ((tmp2 >> 0x10) & 0xffff);
+            *m2++ = ((tmp1 << 0x10) & 0xffff0000) | (tmp2 & 0xffff);
         }
     }
 }
 
-// This function seems to use the SM64 version of the guMtxF2L function
 void guMtxL2F(float mf[4][4], Mtx* m) {
-    int r, c;
+    unsigned int r, c;
     u32 tmp1;
     u32 tmp2;
     u32* m1;
     u32* m2;
     s32 stmp1, stmp2;
-    m1 = (u32*) &m->m[0][0];
-    m2 = (u32*) &m->m[2][0];
+    m1 = (u32*)&m->m[0][0];
+    m2 = (u32*)&m->m[2][0];
     for (r = 0; r < 4; r++) {
         for (c = 0; c < 2; c++) {
             tmp1 = (*m1 & 0xffff0000) | ((*m2 >> 0x10) & 0xffff);
             tmp2 = ((*m1++ << 0x10) & 0xffff0000) | (*m2++ & 0xffff);
-            stmp1 = *(s32*) &tmp1;
-            stmp2 = *(s32*) &tmp2;
+            stmp1 = *(s32*)&tmp1;
+            stmp2 = *(s32*)&tmp2;
             mf[r][c * 2 + 0] = stmp1 / 65536.0f;
             mf[r][c * 2 + 1] = stmp2 / 65536.0f;
         }
     }
 }
 
-void guMtxIdentF(float mf[4][4]) {
-    int i, j;
-
-    for (i = 0; i < 4; i++) {
-        for (j = 0; j < 4; j++) {
-            if (i == j) {
-                mf[i][j] = 1.0;
+void guMtxIdentF(f32 mf[4][4]) {
+    unsigned int r, c;
+    for (r = 0; r < 4; r++) {
+        for (c = 0; c < 4; c++) {
+            if (r == c) {
+                mf[r][c] = 1.0f;
             } else {
-                mf[i][j] = 0.0;
+                mf[r][c] = 0.0f;
             }
         }
     }
@@ -72,4 +68,4 @@ void guMtxIdent(Mtx* m) {
     guMtxIdentF(mf);
 
     guMtxF2L(mf, m);
-}
+}
\ No newline at end of file
diff --git a/src/port/Engine.cpp b/src/port/Engine.cpp
index 1375f958..25ab55ac 100644
--- a/src/port/Engine.cpp
+++ b/src/port/Engine.cpp
@@ -11,9 +11,12 @@
 
 extern "C" {
 float gInterpolationStep = 0.0f;
+#include <sf64thread.h>
+#include <macros.h>
 }
 
 GameEngine* GameEngine::Instance;
+// TimerTask sTimerTasks[0x10];
 
 GameEngine::GameEngine() {
     std::vector<std::string> OTRFiles;
@@ -116,4 +119,71 @@ extern "C" uint32_t GameEngine_GetGameVersion() {
 extern "C" int GameEngine_OTRSigCheck(const char* data) {
     static const char* sOtrSignature = "__OTR__";
     return strncmp(data, sOtrSignature, strlen(sOtrSignature)) == 0;
-}
\ No newline at end of file
+}
+
+extern "C" float SIN_DEG(float angle) {
+    return sinf(M_DTOR * angle);
+}
+extern "C" float COS_DEG(float angle) {
+    return cosf(M_DTOR * angle);
+}
+
+struct TimedEntry {
+    uint64_t duration;
+    TimerAction action;
+    int32_t* address;
+    int32_t value;
+    bool active;
+};
+
+std::vector<TimedEntry> gTimerTasks;
+
+
+uint64_t Timer_GetCurrentMillis() {
+    return SDL_GetTicks();
+}
+
+extern "C" int32_t Timer_CreateTask(uint64_t time, TimerAction action, int32_t* address, int32_t value) {
+    const auto millis = Timer_GetCurrentMillis();
+    TimedEntry entry = {
+        .duration = millis + CYCLES_TO_MSEC_PC(time),
+        .action = action,
+        .address = address,
+        .value = value,
+        .active = true,
+    };
+
+    gTimerTasks.push_back(entry);
+
+    return gTimerTasks.size() - 1;
+}
+
+extern "C" void Timer_Increment(int32_t* address, int32_t value) {
+    *address += value;
+}
+
+extern "C" void Timer_SetValue(int32_t* address, int32_t value) {
+    *address = value;
+}
+
+void Timer_CompleteTask(TimedEntry& task) {
+    if (task.action != nullptr) {
+        task.action(task.address, task.value);
+    }
+    task.active = false;
+}
+
+extern "C" void Timer_Update() {
+
+    if(gTimerTasks.empty()) {
+        return;
+    }
+
+    const auto millis = Timer_GetCurrentMillis();
+
+    for (auto& task : gTimerTasks) {
+        if (task.active && millis >= task.duration) {
+            Timer_CompleteTask(task);
+        }
+    }
+}
diff --git a/src/port/Engine.h b/src/port/Engine.h
index 85c8cd01..0a5998d3 100644
--- a/src/port/Engine.h
+++ b/src/port/Engine.h
@@ -23,7 +23,7 @@ class GameEngine {
     static void RunCommands(Gfx* Commands);
     void ProcessFrame(void (*run_one_game_iter)()) const;
     static void Destroy();
-    void ProcessGfxCommands(Gfx* commands);
+    static void ProcessGfxCommands(Gfx* commands);
 };
 #else
 void GameEngine_ProcessGfxCommands(Gfx* commands);
diff --git a/src/port/Game.cpp b/src/port/Game.cpp
index ca47f686..37abe9c3 100644
--- a/src/port/Game.cpp
+++ b/src/port/Game.cpp
@@ -3,14 +3,26 @@
 #include <Fast3D/gfx_pc.h>
 #include "Engine.h"
 
+extern "C" {
+    void Main_SetVIMode(void);
+    void Main_Initialize(void);
+    void Main_ThreadEntry(void* arg);
+    void Lib_FillScreen(u8 setFill);
+    void Graphics_ThreadUpdate();
+}
+
 extern "C"
-// void exec_display_list(SPTask *spTask) {
-//     GameEngine::ProcessGfxCommands((Gfx *) spTask->task.t.data_ptr);
-// }
+void Graphics_PushFrame(Gfx* data) {
+    GameEngine::ProcessGfxCommands(data);
+}
+
+extern "C" void Timer_Update();
 
 void push_frame() {
     // GameEngine::StartAudioFrame();
     GameEngine::Instance->StartFrame();
+    Graphics_ThreadUpdate();
+    Timer_Update();
     // thread5_iteration();
     // GameEngine::EndAudioFrame();
 }
@@ -21,10 +33,10 @@ int SDL_main(int argc, char **argv) {
 int main(){
 #endif
     GameEngine::Create();
-    // alloc_pool();
-    // audio_init();
-    // sound_init();
-    // thread5_game_loop();
+    Main_SetVIMode();
+    Lib_FillScreen(1);
+    Main_Initialize();
+    Main_ThreadEntry(NULL);
     GameEngine::Instance->ProcessFrame(push_frame);
     GameEngine::Instance->Destroy();
     return 0;
diff --git a/src/sys/sys_main.c b/src/sys/sys_main.c
index 4b0d0147..4842b6e3 100644
--- a/src/sys/sys_main.c
+++ b/src/sys/sys_main.c
@@ -224,10 +224,12 @@ void Timer_ThreadEntry(void* arg0) {
 
     while (1) {
         osRecvMesg(&gTimerTaskMsgQueue, &sp24, OS_MESG_BLOCK);
-        Timer_CompleteTask(sp24.ptr);
+        // Timer_CompleteTask(sp24.ptr);
     }
 }
 
+extern void Graphics_PushFrame(Gfx* masterDL);
+
 void Graphics_ThreadEntry(void* arg0) {
     u8 i;
     u8 var_v1;
@@ -247,44 +249,66 @@ void Graphics_ThreadEntry(void* arg0) {
         gSPEndDisplayList(gMasterDisp++);
     }
     Graphics_SetTask();
-    while (1) {
-        gSysFrameCount++;
-        Graphics_InitializeTask(gSysFrameCount);
-        osRecvMesg(&gControllerMsgQueue, NULL, OS_MESG_BLOCK);
-        osSendMesg(&gSerialThreadMsgQueue, OS_MESG_32(SI_RUMBLE), OS_MESG_PRI_NORMAL);
-        Controller_UpdateInput();
-        osSendMesg(&gSerialThreadMsgQueue, OS_MESG_32(SI_READ_CONTROLLER), OS_MESG_PRI_NORMAL);
-        if (gControllerPress[3].button & U_JPAD) {
-            Main_SetVIMode();
-        }
-        {
-            gSPSegment(gUnkDisp1++, 0, 0);
-            gSPDisplayList(gMasterDisp++, gGfxPool->unkDL1);
-            Game_Update();
-            if (gStartNMI == 1) {
-                Graphics_NMIWipe();
-            }
-            gSPEndDisplayList(gUnkDisp1++);
-            gSPEndDisplayList(gUnkDisp2++);
-            gSPDisplayList(gMasterDisp++, gGfxPool->unkDL2);
-            gDPFullSync(gMasterDisp++);
-            gSPEndDisplayList(gMasterDisp++);
-        }
-        osRecvMesg(&gGfxTaskMsgQueue, NULL, OS_MESG_BLOCK);
-        Graphics_SetTask();
-        if (gFillScreen == 0) {
-            osViSwapBuffer(&gFrameBuffers[(gSysFrameCount - 1) % 3]);
-        }
-        // func_80007FE4(&gFrameBuffers[(gSysFrameCount - 1) % 3], SCREEN_WIDTH, 16);
+    // while (1) {
 
-        var_v1 = MIN(D_80137E78, 4);
-        var_v2 = MAX(var_v1, gGfxVImsgQueue.validCount + 1);
-        for (i = 0; i < var_v2; i += 1) { // Can't be ++
-            osRecvMesg(&gGfxVImsgQueue, NULL, OS_MESG_BLOCK);
-        }
+    // }
+}
 
-        Audio_Update();
+void Graphics_ThreadUpdate(){
+
+    if (GfxDebuggerIsDebugging()) {
+        Graphics_PushFrame(gGfxPool->masterDL);
+        return;
     }
+
+    gSysFrameCount++;
+    Graphics_InitializeTask(gSysFrameCount);
+    osRecvMesg(&gControllerMsgQueue, NULL, OS_MESG_BLOCK);
+    osSendMesg(&gSerialThreadMsgQueue, OS_MESG_32(SI_RUMBLE), OS_MESG_PRI_NORMAL);
+    Controller_UpdateInput();
+    osSendMesg(&gSerialThreadMsgQueue, OS_MESG_32(SI_READ_CONTROLLER), OS_MESG_PRI_NORMAL);
+    if (gControllerPress[3].button & U_JPAD) {
+        Main_SetVIMode();
+    }
+    {
+        gSPSegment(gUnkDisp1++, 0, 0);
+        gSPDisplayList(gMasterDisp++, gGfxPool->unkDL1);
+        Game_Update();
+        if (gStartNMI == 1) {
+            Graphics_NMIWipe();
+        }
+        gSPEndDisplayList(gUnkDisp1++);
+        gSPEndDisplayList(gUnkDisp2++);
+        gSPDisplayList(gMasterDisp++, gGfxPool->unkDL2);
+        gDPFullSync(gMasterDisp++);
+        gSPEndDisplayList(gMasterDisp++);
+    }
+    osRecvMesg(&gGfxTaskMsgQueue, NULL, OS_MESG_BLOCK);
+    Graphics_SetTask();
+
+    if(GfxDebuggerIsDebuggingRequested()) {
+        GfxDebuggerDebugDisplayList(gGfxPool->masterDL);
+    }
+
+    Graphics_PushFrame(gGfxPool->masterDL);
+
+    if (gFillScreen == 0) {
+        osViSwapBuffer(&gFrameBuffers[(gSysFrameCount - 1) % 3]);
+    }
+
+    Controller_ReadData();
+    // LTODO: FAULT_CRASH
+    // func_80007FE4(&gFrameBuffers[(gSysFrameCount - 1) % 3], SCREEN_WIDTH, 16);
+
+    // LTODO: Figure out what this is
+    // var_v1 = MIN(D_80137E78, 4);
+    // var_v2 = MAX(var_v1, gGfxVImsgQueue.validCount + 1);
+    // for (i = 0; i < var_v2; i += 1) { // Can't be ++
+    //     osRecvMesg(&gGfxVImsgQueue, NULL, OS_MESG_BLOCK);
+    // }
+
+    // LTODO: There is no audio for now :P
+    // Audio_Update();
 }
 
 void Main_InitMesgQueues(void) {
@@ -424,45 +448,41 @@ void Main_ThreadEntry(void* arg0) {
     OSMesg ogMsg;
     u32 mesg;
 
-    // osCreateThread(&gAudioThread, THREAD_ID_AUDIO, Audio_ThreadEntry, arg0,
-    //                gAudioThreadStack + sizeof(gAudioThreadStack), 80);
-    // osStartThread(&gAudioThread);
-    // osCreateThread(&gGraphicsThread, THREAD_ID_GRAPHICS, Graphics_ThreadEntry, arg0,
-    //                gGraphicsThreadStack + sizeof(gGraphicsThreadStack), 40);
-    // osStartThread(&gGraphicsThread);
-    // osCreateThread(&gTimerThread, THREAD_ID_TIMER, Timer_ThreadEntry, arg0,
-    //                gTimerThreadStack + sizeof(gTimerThreadStack), 60);
-    // osStartThread(&gTimerThread);
-    // osCreateThread(&gSerialThread, THREAD_ID_SERIAL, SerialInterface_ThreadEntry, arg0,
-    //                gSerialThreadStack + sizeof(gSerialThreadStack), 20);
-    // osStartThread(&gSerialThread);
+    // LTODO: Implement audio
+    // Audio_ThreadEntry(NULL);
+    Graphics_ThreadEntry(NULL);
+    Controller_Init();
 
-    Main_InitMesgQueues();
+    // LTODO: Implement timers
+    // Timer_ThreadEntry(NULL);
 
-    while (true) {
-        osRecvMesg(&gMainThreadMsgQueue, &ogMsg, OS_MESG_BLOCK);
-        mesg = ogMsg.data32;
 
-        switch (mesg) {
-            case EVENT_MESG_VI:
-                osSendMesg(&gAudioVImsgQueue, OS_MESG_32(EVENT_MESG_VI), OS_MESG_PRI_NORMAL);
-                osSendMesg(&gGfxVImsgQueue, OS_MESG_32(EVENT_MESG_VI), OS_MESG_PRI_NORMAL);
-                Main_GetNewTasks();
-                break;
-            case EVENT_MESG_SP:
-                Main_HandleRSP();
-                break;
-            case EVENT_MESG_DP:
-                Main_HandleRDP();
-                break;
-            case EVENT_MESG_PRENMI:
-                gStartNMI = 1;
-                break;
-        }
-        if (gStopTasks == 0) {
-            Main_StartNextTask();
-        }
-    }
+    // N64 Stuff should not be needed
+    // Main_InitMesgQueues();
+    // while (true) {
+    //     osRecvMesg(&gMainThreadMsgQueue, &ogMsg, OS_MESG_BLOCK);
+    //     mesg = ogMsg.data32;
+
+    //     switch (mesg) {
+    //         case EVENT_MESG_VI:
+    //             osSendMesg(&gAudioVImsgQueue, OS_MESG_32(EVENT_MESG_VI), OS_MESG_PRI_NORMAL);
+    //             osSendMesg(&gGfxVImsgQueue, OS_MESG_32(EVENT_MESG_VI), OS_MESG_PRI_NORMAL);
+    //             Main_GetNewTasks();
+    //             break;
+    //         case EVENT_MESG_SP:
+    //             Main_HandleRSP();
+    //             break;
+    //         case EVENT_MESG_DP:
+    //             Main_HandleRDP();
+    //             break;
+    //         case EVENT_MESG_PRENMI:
+    //             gStartNMI = 1;
+    //             break;
+    //     }
+    //     if (gStopTasks == 0) {
+    //         Main_StartNextTask();
+    //     }
+    // }
 }
 
 void Idle_ThreadEntry(void* arg0) {
diff --git a/src/sys/sys_matrix.c b/src/sys/sys_matrix.c
index 2722bfb5..9b2a1654 100644
--- a/src/sys/sys_matrix.c
+++ b/src/sys/sys_matrix.c
@@ -1,19 +1,28 @@
 #include "sys.h"
 
-Mtx gIdentityMtx = {
-    .intPart = {
-        { 1, 0, 0, 0 },
-        { 0, 1, 0, 0 },
-        { 0, 0, 1, 0 },
-        { 0, 0, 0, 1 },
-    },
-    .fracPart = {
-        { 0, 0, 0, 0 },
-        { 0, 0, 0, 0 },
-        { 0, 0, 0, 0 },
-        { 0, 0, 0, 0 },
-    },
-};
+#define qs1616(e) ((s32)((e)*0x00010000))
+
+#define IPART(x) ((qs1616(x) >> 16) & 0xFFFF)
+#define FPART(x) (qs1616(x) & 0xFFFF)
+
+#define gdSPDefMtx(xx, yx, zx, wx, xy, yy, zy, wy, xz, yz, zz, wz, xw, yw, zw, ww)                                 \
+    {                                                                                                              \
+        {                                                                                                          \
+            (IPART(xx) << 0x10) | IPART(xy), (IPART(xz) << 0x10) | IPART(xw), (IPART(yx) << 0x10) | IPART(yy),     \
+                (IPART(yz) << 0x10) | IPART(yw), (IPART(zx) << 0x10) | IPART(zy), (IPART(zz) << 0x10) | IPART(zw), \
+                (IPART(wx) << 0x10) | IPART(wy), (IPART(wz) << 0x10) | IPART(ww), (FPART(xx) << 0x10) | FPART(xy), \
+                (FPART(xz) << 0x10) | FPART(xw), (FPART(yx) << 0x10) | FPART(yy), (FPART(yz) << 0x10) | FPART(yw), \
+                (FPART(zx) << 0x10) | FPART(zy), (FPART(zz) << 0x10) | FPART(zw), (FPART(wx) << 0x10) | FPART(wy), \
+                (FPART(wz) << 0x10) | FPART(ww),                                                                   \
+        }                                                                                                          \
+    }
+
+Mtx gIdentityMtx = gdSPDefMtx(
+    1.0f, 0.0f, 0.0f, 0.0f,
+    0.0f, 1.0f, 0.0f, 0.0f,
+    0.0f, 0.0f, 1.0f, 0.0f,
+    0.0f, 0.0f, 0.0f, 1.0f
+);
 
 Matrix gIdentityMatrix = { {
     { 1.0f, 0.0f, 0.0f, 0.0f },
@@ -23,9 +32,9 @@ Matrix gIdentityMatrix = { {
 } };
 
 Matrix* gGfxMatrix;
-Matrix sGfxMatrixStack[0x20];
+Matrix sGfxMatrixStack[0x150];
 Matrix* gCalcMatrix;
-Matrix sCalcMatrixStack[0x20];
+Matrix sCalcMatrixStack[0x150];
 
 // Copies src Matrix into dst
 void Matrix_Copy(Matrix* dst, Matrix* src) {
@@ -50,375 +59,559 @@ void Matrix_Pop(Matrix** mtxStack) {
     *mtxStack -= 1;
 }
 
-// Copies tf into mtx (MTXMODE_NEW) or applies it to mtx (MTXMODE_APPLY)
-void Matrix_Mult(Matrix* mtx, Matrix* tf, u8 mode) {
+void MtxFMtxFMult(MtxF* mfB, MtxF* mfA, MtxF* dest) {
     f32 rx;
     f32 ry;
     f32 rz;
     f32 rw;
-    s32 i0;
-    s32 i1;
-    s32 i2;
-    s32 i3;
+
+    //---COL1---
+    f32 cx = mfB->xx;
+    f32 cy = mfB->xy;
+    f32 cz = mfB->xz;
+    f32 cw = mfB->xw;
+    //--------
+
+    rx = mfA->xx;
+    ry = mfA->yx;
+    rz = mfA->zx;
+    rw = mfA->wx;
+    dest->xx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xy;
+    ry = mfA->yy;
+    rz = mfA->zy;
+    rw = mfA->wy;
+    dest->xy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xz;
+    ry = mfA->yz;
+    rz = mfA->zz;
+    rw = mfA->wz;
+    dest->xz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xw;
+    ry = mfA->yw;
+    rz = mfA->zw;
+    rw = mfA->ww;
+    dest->xw = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    //---2Col---
+    cx = mfB->yx;
+    cy = mfB->yy;
+    cz = mfB->yz;
+    cw = mfB->yw;
+    //--------
+    rx = mfA->xx;
+    ry = mfA->yx;
+    rz = mfA->zx;
+    rw = mfA->wx;
+    dest->yx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xy;
+    ry = mfA->yy;
+    rz = mfA->zy;
+    rw = mfA->wy;
+    dest->yy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xz;
+    ry = mfA->yz;
+    rz = mfA->zz;
+    rw = mfA->wz;
+    dest->yz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xw;
+    ry = mfA->yw;
+    rz = mfA->zw;
+    rw = mfA->ww;
+    dest->yw = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    //---3Col---
+    cx = mfB->zx;
+    cy = mfB->zy;
+    cz = mfB->zz;
+    cw = mfB->zw;
+    //--------
+    rx = mfA->xx;
+    ry = mfA->yx;
+    rz = mfA->zx;
+    rw = mfA->wx;
+    dest->zx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xy;
+    ry = mfA->yy;
+    rz = mfA->zy;
+    rw = mfA->wy;
+    dest->zy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xz;
+    ry = mfA->yz;
+    rz = mfA->zz;
+    rw = mfA->wz;
+    dest->zz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xw;
+    ry = mfA->yw;
+    rz = mfA->zw;
+    rw = mfA->ww;
+    dest->zw = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    //---4Col---
+    cx = mfB->wx;
+    cy = mfB->wy;
+    cz = mfB->wz;
+    cw = mfB->ww;
+    //--------
+    rx = mfA->xx;
+    ry = mfA->yx;
+    rz = mfA->zx;
+    rw = mfA->wx;
+    dest->wx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xy;
+    ry = mfA->yy;
+    rz = mfA->zy;
+    rw = mfA->wy;
+    dest->wy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xz;
+    ry = mfA->yz;
+    rz = mfA->zz;
+    rw = mfA->wz;
+    dest->wz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xw;
+    ry = mfA->yw;
+    rz = mfA->zw;
+    rw = mfA->ww;
+    dest->ww = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+}
+
+void Matrix_MtxFCopy(MtxF* dest, MtxF* src) {
+    dest->xx = src->xx;
+    dest->yx = src->yx;
+    dest->zx = src->zx;
+    dest->wx = src->wx;
+    dest->xy = src->xy;
+    dest->yy = src->yy;
+    dest->zy = src->zy;
+    dest->wy = src->wy;
+    dest->xx = src->xx;
+    dest->yx = src->yx;
+    dest->zx = src->zx;
+    dest->wx = src->wx;
+    dest->xy = src->xy;
+    dest->yy = src->yy;
+    dest->zy = src->zy;
+    dest->wy = src->wy;
+    dest->xz = src->xz;
+    dest->yz = src->yz;
+    dest->zz = src->zz;
+    dest->wz = src->wz;
+    dest->xw = src->xw;
+    dest->yw = src->yw;
+    dest->zw = src->zw;
+    dest->ww = src->ww;
+    dest->xz = src->xz;
+    dest->yz = src->yz;
+    dest->zz = src->zz;
+    dest->wz = src->wz;
+    dest->xw = src->xw;
+    dest->yw = src->yw;
+    dest->zw = src->zw;
+    dest->ww = src->ww;
+}
+
+// Copies tf into mtx (MTXMODE_NEW) or applies it to mtx (MTXMODE_APPLY)
+void Matrix_Mult(Matrix* mtx, Matrix* tf, u8 mode) {
+    MtxF* cmf = mtx->m;
 
     if (mode == 1) {
-        rx = mtx->m[0][0];
-        ry = mtx->m[1][0];
-        rz = mtx->m[2][0];
-        rw = mtx->m[3][0];
-
-        for (i0 = 0; i0 < 4; i0++) {
-            mtx->m[i0][0] = (rx * tf->m[i0][0]) + (ry * tf->m[i0][1]) + (rz * tf->m[i0][2]) + (rw * tf->m[i0][3]);
-        }
-
-        rx = mtx->m[0][1];
-        ry = mtx->m[1][1];
-        rz = mtx->m[2][1];
-        rw = mtx->m[3][1];
-
-        for (i1 = 0; i1 < 4; i1++) {
-            mtx->m[i1][1] = (rx * tf->m[i1][0]) + (ry * tf->m[i1][1]) + (rz * tf->m[i1][2]) + (rw * tf->m[i1][3]);
-        }
-
-        rx = mtx->m[0][2];
-        ry = mtx->m[1][2];
-        rz = mtx->m[2][2];
-        rw = mtx->m[3][2];
-
-        for (i2 = 0; i2 < 4; i2++) {
-            mtx->m[i2][2] = (rx * tf->m[i2][0]) + (ry * tf->m[i2][1]) + (rz * tf->m[i2][2]) + (rw * tf->m[i2][3]);
-        }
-
-        rx = mtx->m[0][3];
-        ry = mtx->m[1][3];
-        rz = mtx->m[2][3];
-        rw = mtx->m[3][3];
-
-        for (i3 = 0; i3 < 4; i3++) {
-            mtx->m[i3][3] = (rx * tf->m[i3][0]) + (ry * tf->m[i3][1]) + (rz * tf->m[i3][2]) + (rw * tf->m[i3][3]);
-        }
+        MtxFMtxFMult(cmf, tf->m, cmf);
     } else {
-        Matrix_Copy(mtx, tf);
+        Matrix_MtxFCopy(cmf, tf->m);
     }
 }
 
 // Creates a translation matrix in mtx (MTXMODE_NEW) or applies one to mtx (MTXMODE_APPLY)
 void Matrix_Translate(Matrix* mtx, f32 x, f32 y, f32 z, u8 mode) {
-    f32 rx;
-    f32 ry;
-    s32 i;
+    MtxF* cmf = mtx->m;
+    f32 tempX;
+    f32 tempY;
 
     if (mode == 1) {
-        for (i = 0; i < 4; i++) {
-            rx = mtx->m[0][i];
-            ry = mtx->m[1][i];
-
-            mtx->m[3][i] += (rx * x) + (ry * y) + (mtx->m[2][i] * z);
-        }
+        tempX = cmf->xx;
+        tempY = cmf->xy;
+        cmf->xw += tempX * x + tempY * y + cmf->xz * z;
+        tempX = cmf->yx;
+        tempY = cmf->yy;
+        cmf->yw += tempX * x + tempY * y + cmf->yz * z;
+        tempX = cmf->zx;
+        tempY = cmf->zy;
+        cmf->zw += tempX * x + tempY * y + cmf->zz * z;
+        tempX = cmf->wx;
+        tempY = cmf->wy;
+        cmf->ww += tempX * x + tempY * y + cmf->wz * z;
     } else {
-        mtx->m[3][0] = x;
-        mtx->m[3][1] = y;
-        mtx->m[3][2] = z;
-        mtx->m[0][1] = mtx->m[0][2] = mtx->m[0][3] = mtx->m[1][0] = mtx->m[1][2] = mtx->m[1][3] = mtx->m[2][0] =
-            mtx->m[2][1] = mtx->m[2][3] = 0.0f;
-        mtx->m[0][0] = mtx->m[1][1] = mtx->m[2][2] = mtx->m[3][3] = 1.0f;
+        cmf->yx = 0.0f;
+        cmf->zx = 0.0f;
+        cmf->wx = 0.0f;
+        cmf->xy = 0.0f;
+        cmf->zy = 0.0f;
+        cmf->wy = 0.0f;
+        cmf->xz = 0.0f;
+        cmf->yz = 0.0f;
+        cmf->wz = 0.0f;
+        cmf->xx = 1.0f;
+        cmf->yy = 1.0f;
+        cmf->zz = 1.0f;
+        cmf->ww = 1.0f;
+        cmf->xw = x;
+        cmf->yw = y;
+        cmf->zw = z;
     }
 }
 
 // Creates a scale matrix in mtx (MTXMODE_NEW) or applies one to mtx (MTXMODE_APPLY)
-void Matrix_Scale(Matrix* mtx, f32 xScale, f32 yScale, f32 zScale, u8 mode) {
-    f32 rx;
-    f32 ry;
-    s32 i;
+void Matrix_Scale(Matrix* mtx, f32 x, f32 y, f32 z, u8 mode) {
+    MtxF* cmf = mtx->m;
 
     if (mode == 1) {
-        for (i = 0; i < 4; i++) {
-            rx = mtx->m[0][i];
-            ry = mtx->m[1][i];
-
-            mtx->m[0][i] = rx * xScale;
-            mtx->m[1][i] = ry * yScale;
-            mtx->m[2][i] *= zScale;
-        }
+        cmf->xx *= x;
+        cmf->yx *= x;
+        cmf->zx *= x;
+        cmf->xy *= y;
+        cmf->yy *= y;
+        cmf->zy *= y;
+        cmf->xz *= z;
+        cmf->yz *= z;
+        cmf->zz *= z;
+        cmf->wx *= x;
+        cmf->wy *= y;
+        cmf->wz *= z;
     } else {
-        mtx->m[0][0] = xScale;
-        mtx->m[1][1] = yScale;
-        mtx->m[2][2] = zScale;
-        mtx->m[0][1] = mtx->m[0][2] = mtx->m[0][3] = mtx->m[1][0] = mtx->m[1][2] = mtx->m[1][3] = mtx->m[2][0] =
-            mtx->m[2][1] = mtx->m[2][3] = mtx->m[3][0] = mtx->m[3][1] = mtx->m[3][2] = 0.0f;
-        mtx->m[3][3] = 1.0f;
+        cmf->yx = 0.0f;
+        cmf->zx = 0.0f;
+        cmf->wx = 0.0f;
+        cmf->xy = 0.0f;
+        cmf->zy = 0.0f;
+        cmf->wy = 0.0f;
+        cmf->xz = 0.0f;
+        cmf->yz = 0.0f;
+        cmf->wz = 0.0f;
+        cmf->xw = 0.0f;
+        cmf->yw = 0.0f;
+        cmf->zw = 0.0f;
+        cmf->ww = 1.0f;
+        cmf->xx = x;
+        cmf->yy = y;
+        cmf->zz = z;
     }
 }
 
 // Creates rotation matrix about the X axis in mtx (MTXMODE_NEW) or applies one to mtx (MTXMODE_APPLY)
-void Matrix_RotateX(Matrix* mtx, f32 angle, u8 mode) {
-    f32 cs;
-    f32 sn;
-    f32 ry;
-    f32 rz;
-    s32 i;
+void Matrix_RotateX(Matrix* mtx, f32 x, u8 mode) {
+    MtxF* cmf;
+    f32 sin;
+    f32 cos;
+    f32 tempY;
+    f32 tempZ;
+    f32 zero = 0.0;
+    f32 one = 1.0;
 
-    sn = __sinf(angle);
-    cs = __cosf(angle);
     if (mode == 1) {
-        for (i = 0; i < 4; i++) {
-            ry = mtx->m[1][i];
-            rz = mtx->m[2][i];
+        if (x != 0) {
+            cmf = mtx->m;
 
-            mtx->m[1][i] = (ry * cs) + (rz * sn);
-            mtx->m[2][i] = (rz * cs) - (ry * sn);
+            sin = sinf(x);
+            cos = cosf(x);
+
+            tempY = cmf->xy;
+            tempZ = cmf->xz;
+            cmf->xy = tempY * cos + tempZ * sin;
+            cmf->xz = tempZ * cos - tempY * sin;
+
+            tempY = cmf->yy;
+            tempZ = cmf->yz;
+            cmf->yy = tempY * cos + tempZ * sin;
+            cmf->yz = tempZ * cos - tempY * sin;
+
+            tempY = cmf->zy;
+            tempZ = cmf->zz;
+            cmf->zy = tempY * cos + tempZ * sin;
+            cmf->zz = tempZ * cos - tempY * sin;
+
+            tempY = cmf->wy;
+            tempZ = cmf->wz;
+            cmf->wy = tempY * cos + tempZ * sin;
+            cmf->wz = tempZ * cos - tempY * sin;
         }
     } else {
-        mtx->m[1][1] = mtx->m[2][2] = cs;
-        mtx->m[1][2] = sn;
-        mtx->m[2][1] = -sn;
-        mtx->m[0][0] = mtx->m[3][3] = 1.0f;
-        mtx->m[0][1] = mtx->m[0][2] = mtx->m[0][3] = mtx->m[1][0] = mtx->m[1][3] = mtx->m[2][0] = mtx->m[2][3] =
-            mtx->m[3][0] = mtx->m[3][1] = mtx->m[3][2] = 0.0f;
+        cmf = mtx->m;
+
+        if (x != 0) {
+            sin = sinf(x);
+            cos = cosf(x);
+        } else {
+            sin = zero;
+            cos = one;
+        }
+
+        cmf->xx = one;
+        cmf->yx = zero;
+        cmf->zx = zero;
+        cmf->wx = zero;
+        cmf->xy = zero;
+        cmf->yy = cos;
+        cmf->zy = sin;
+        cmf->wy = zero;
+        cmf->xz = zero;
+        cmf->yz = -sin;
+        cmf->zz = cos;
+        cmf->wz = zero;
+        cmf->xw = zero;
+        cmf->yw = zero;
+        cmf->zw = zero;
+        cmf->ww = one;
     }
 }
 
 // Creates rotation matrix about the Y axis in mtx (MTXMODE_NEW) or applies one to mtx (MTXMODE_APPLY)
-void Matrix_RotateY(Matrix* mtx, f32 angle, u8 mode) {
-    f32 cs;
-    f32 sn;
-    f32 rx;
-    f32 rz;
-    s32 i;
+void Matrix_RotateY(Matrix* mtx, f32 y, u8 mode) {
+    MtxF* cmf;
+    f32 sin;
+    f32 cos;
+    f32 tempX;
+    f32 tempZ;
+    f32 zero = 0.0;
+    f32 one = 1.0;
 
-    sn = __sinf(angle);
-    cs = __cosf(angle);
     if (mode == 1) {
-        for (i = 0; i < 4; i++) {
-            rx = mtx->m[0][i];
-            rz = mtx->m[2][i];
+        if (y != 0.0f) {
+            cmf = mtx->m;
 
-            mtx->m[0][i] = (rx * cs) - (rz * sn);
-            mtx->m[2][i] = (rx * sn) + (rz * cs);
+            sin = sinf(y);
+            cos = cosf(y);
+
+            tempX = cmf->xx;
+            tempZ = cmf->xz;
+            cmf->xx = tempX * cos - tempZ * sin;
+            cmf->xz = tempX * sin + tempZ * cos;
+
+            tempX = cmf->yx;
+            tempZ = cmf->yz;
+            cmf->yx = tempX * cos - tempZ * sin;
+            cmf->yz = tempX * sin + tempZ * cos;
+
+            tempX = cmf->zx;
+            tempZ = cmf->zz;
+            cmf->zx = tempX * cos - tempZ * sin;
+            cmf->zz = tempX * sin + tempZ * cos;
+
+            tempX = cmf->wx;
+            tempZ = cmf->wz;
+            cmf->wx = tempX * cos - tempZ * sin;
+            cmf->wz = tempX * sin + tempZ * cos;
         }
     } else {
-        mtx->m[0][0] = mtx->m[2][2] = cs;
-        mtx->m[0][2] = -sn;
-        mtx->m[2][0] = sn;
-        mtx->m[1][1] = mtx->m[3][3] = 1.0f;
-        mtx->m[0][1] = mtx->m[0][3] = mtx->m[1][0] = mtx->m[1][2] = mtx->m[1][3] = mtx->m[2][1] = mtx->m[2][3] =
-            mtx->m[3][0] = mtx->m[3][1] = mtx->m[3][2] = 0.0f;
+        cmf = mtx->m;
+
+        if (y != 0.0f) {
+            sin = sinf(y);
+            cos = cosf(y);
+        } else {
+            cos = one;
+            sin = zero;
+        }
+
+        cmf->yx = zero;
+        cmf->wx = zero;
+        cmf->xy = zero;
+        cmf->zy = zero;
+        cmf->wy = zero;
+        cmf->yz = zero;
+        cmf->wz = zero;
+        cmf->xw = zero;
+        cmf->yw = zero;
+        cmf->zw = zero;
+        cmf->yy = one;
+        cmf->ww = one;
+        cmf->xx = cos;
+        cmf->zz = cos;
+        cmf->zx = -sin;
+        cmf->xz = sin;
     }
 }
 
 // Creates rotation matrix about the Z axis in mtx (MTXMODE_NEW) or applies one to mtx (MTXMODE_APPLY)
-void Matrix_RotateZ(Matrix* mtx, f32 angle, u8 mode) {
-    f32 cs;
-    f32 sn;
-    f32 rx;
-    f32 ry;
-    s32 i;
+void Matrix_RotateZ(Matrix* mtx, f32 z, u8 mode) {
+    MtxF* cmf;
+    f32 sin;
+    f32 cos;
+    f32 tempX;
+    f32 tempY;
 
-    sn = __sinf(angle);
-    cs = __cosf(angle);
     if (mode == 1) {
-        for (i = 0; i < 4; i++) {
-            rx = mtx->m[0][i];
-            ry = mtx->m[1][i];
+        if (z != 0) {
+            cmf = mtx->m;
 
-            mtx->m[0][i] = (rx * cs) + (ry * sn);
-            mtx->m[1][i] = (ry * cs) - (rx * sn);
+            sin = sinf(z);
+            cos = cosf(z);
+
+            tempX = cmf->xx;
+            tempY = cmf->xy;
+            cmf->xx = tempX * cos + tempY * sin;
+            cmf->xy = tempY * cos - tempX * sin;
+
+            tempX = cmf->yx;
+            tempY = cmf->yy;
+            cmf->yx = tempX * cos + tempY * sin;
+            cmf->yy = tempY * cos - tempX * sin;
+
+            tempX = cmf->zx;
+            tempY = cmf->zy;
+            cmf->zx = tempX * cos + tempY * sin;
+            cmf->zy = tempY * cos - tempX * sin;
+
+            tempX = cmf->wx;
+            tempY = cmf->wy;
+            cmf->wx = tempX * cos + tempY * sin;
+            cmf->wy = tempY * cos - tempX * sin;
         }
     } else {
-        mtx->m[0][0] = mtx->m[1][1] = cs;
-        mtx->m[0][1] = sn;
-        mtx->m[1][0] = -sn;
-        mtx->m[2][2] = mtx->m[3][3] = 1.0f;
-        mtx->m[0][2] = mtx->m[0][3] = mtx->m[1][2] = mtx->m[1][3] = mtx->m[2][0] = mtx->m[2][1] = mtx->m[2][3] =
-            mtx->m[3][0] = mtx->m[3][1] = mtx->m[3][2] = 0.0f;
+        cmf = mtx->m;
+
+        if (z != 0) {
+            sin = sinf(z);
+            cos = cosf(z);
+        } else {
+            sin = 0.0f;
+            cos = 1.0f;
+        }
+
+        cmf->zx = 0.0f;
+        cmf->wx = 0.0f;
+        cmf->zy = 0.0f;
+        cmf->wy = 0.0f;
+        cmf->xz = 0.0f;
+        cmf->yz = 0.0f;
+        cmf->wz = 0.0f;
+        cmf->xw = 0.0f;
+        cmf->yw = 0.0f;
+        cmf->zw = 0.0f;
+        cmf->zz = 1.0f;
+        cmf->ww = 1.0f;
+        cmf->xx = cos;
+        cmf->yy = cos;
+        cmf->yx = sin;
+        cmf->xy = -sin;
     }
 }
 
 // Creates rotation matrix about a given vector axis in mtx (MTXMODE_NEW) or applies one to mtx (MTXMODE_APPLY).
 // The vector specifying the axis does not need to be a unit vector.
-void Matrix_RotateAxis(Matrix* mtx, f32 angle, f32 axisX, f32 axisY, f32 axisZ, u8 mode) {
-    f32 rx;
-    f32 ry;
-    f32 rz;
-    f32 norm;
-    f32 cxx;
-    f32 cyx;
-    f32 czx;
-    f32 cxy;
-    f32 cyy;
-    f32 czy;
-    f32 cxz;
-    f32 cyz;
-    f32 czz;
-    f32 xx;
-    f32 yy;
-    f32 zz;
-    f32 xy;
-    f32 yz;
-    f32 xz;
-    f32 sinA;
-    f32 cosA;
+void Matrix_RotateAxis(Matrix* mtx, f32 angle, f32 x, f32 y, f32 z, u8 mode) {
+    MtxF* cmf;
+    f32 sin;
+    f32 cos;
+    f32 versin;
+    f32 temp1;
+    f32 temp2;
+    f32 temp3;
+    f32 temp4;
 
-    norm = sqrtf((axisX * axisX) + (axisY * axisY) + (axisZ * axisZ));
-    if (norm != 0.0) {
-        axisX /= norm;
-        axisY /= norm;
-        axisZ /= norm;
-        sinA = __sinf(angle);
-        cosA = __cosf(angle);
-        xx = axisX * axisX;
-        yy = axisY * axisY;
-        zz = axisZ * axisZ;
-        xy = axisX * axisY;
-        yz = axisY * axisZ;
-        xz = axisX * axisZ;
+    if (mode == 1) {
+        if (angle != 0) {
+            cmf = mtx->m;
 
-        if (mode == 1) {
-            cxx = (1.0f - xx) * cosA + xx;
-            cyx = (1.0f - cosA) * xy + axisZ * sinA;
-            czx = (1.0f - cosA) * xz - axisY * sinA;
+            sin = sinf(angle);
+            cos = cosf(angle);
 
-            cxy = (1.0f - cosA) * xy - axisZ * sinA;
-            cyy = (1.0f - yy) * cosA + yy;
-            czy = (1.0f - cosA) * yz + axisX * sinA;
+            temp1 = cmf->xx;
+            temp2 = cmf->xy;
+            temp3 = cmf->xz;
+            temp4 = (x * temp1 + y * temp2 + z * temp3) * (1.0f - cos);
+            cmf->xx = temp1 * cos + x * temp4 + sin * (temp2 * z - temp3 * y);
+            cmf->xy = temp2 * cos + y * temp4 + sin * (temp3 * x - temp1 * z);
+            cmf->xz = temp3 * cos + z * temp4 + sin * (temp1 * y - temp2 * x);
 
-            cxz = (1.0f - cosA) * xz + axisY * sinA;
-            cyz = (1.0f - cosA) * yz - axisX * sinA;
-            czz = (1.0f - zz) * cosA + zz;
+            temp1 = cmf->yx;
+            temp2 = cmf->yy;
+            temp3 = cmf->yz;
+            temp4 = (x * temp1 + y * temp2 + z * temp3) * (1.0f - cos);
+            cmf->yx = temp1 * cos + x * temp4 + sin * (temp2 * z - temp3 * y);
+            cmf->yy = temp2 * cos + y * temp4 + sin * (temp3 * x - temp1 * z);
+            cmf->yz = temp3 * cos + z * temp4 + sin * (temp1 * y - temp2 * x);
 
-            // loop doesn't seem to work here.
-            rx = mtx->m[0][0];
-            ry = mtx->m[0][1];
-            rz = mtx->m[0][2];
-            mtx->m[0][0] = (rx * cxx) + (ry * cxy) + (rz * cxz);
-            mtx->m[0][1] = (rx * cyx) + (ry * cyy) + (rz * cyz);
-            mtx->m[0][2] = (rx * czx) + (ry * czy) + (rz * czz);
+            temp1 = cmf->zx;
+            temp2 = cmf->zy;
+            temp3 = cmf->zz;
+            temp4 = (x * temp1 + y * temp2 + z * temp3) * (1.0f - cos);
+            cmf->zx = temp1 * cos + x * temp4 + sin * (temp2 * z - temp3 * y);
+            cmf->zy = temp2 * cos + y * temp4 + sin * (temp3 * x - temp1 * z);
+            cmf->zz = temp3 * cos + z * temp4 + sin * (temp1 * y - temp2 * x);
+        }
+    } else {
+        cmf = mtx->m;
 
-            rx = mtx->m[1][0];
-            ry = mtx->m[1][1];
-            rz = mtx->m[1][2];
-            mtx->m[1][0] = (rx * cxx) + (ry * cxy) + (rz * cxz);
-            mtx->m[1][1] = (rx * cyx) + (ry * cyy) + (rz * cyz);
-            mtx->m[1][2] = (rx * czx) + (ry * czy) + (rz * czz);
+        if (angle != 0) {
+            sin = sinf(angle);
+            cos = cosf(angle);
+            versin = 1.0f - cos;
 
-            rx = mtx->m[2][0];
-            ry = mtx->m[2][1];
-            rz = mtx->m[2][2];
-            mtx->m[2][0] = (rx * cxx) + (ry * cxy) + (rz * cxz);
-            mtx->m[2][1] = (rx * cyx) + (ry * cyy) + (rz * cyz);
-            mtx->m[2][2] = (rx * czx) + (ry * czy) + (rz * czz);
+            cmf->xx = x * x * versin + cos;
+            cmf->yy = y * y * versin + cos;
+            cmf->zz = z * z * versin + cos;
+
+            if (0) {}
+
+            temp2 = x * versin * y;
+            temp3 = z * sin;
+            cmf->yx = temp2 + temp3;
+            cmf->xy = temp2 - temp3;
+
+            temp2 = x * versin * z;
+            temp3 = y * sin;
+            cmf->zx = temp2 - temp3;
+            cmf->xz = temp2 + temp3;
+
+            temp2 = y * versin * z;
+            temp3 = x * sin;
+            cmf->zy = temp2 + temp3;
+            cmf->yz = temp2 - temp3;
+
+            cmf->wx = cmf->wy = cmf->wz = cmf->xw = cmf->yw = cmf->zw = 0.0f;
+            cmf->ww = 1.0f;
         } else {
-            mtx->m[0][0] = (1.0f - xx) * cosA + xx;
-            mtx->m[0][1] = (1.0f - cosA) * xy + axisZ * sinA;
-            mtx->m[0][2] = (1.0f - cosA) * xz - axisY * sinA;
-            mtx->m[0][3] = 0.0f;
-
-            mtx->m[1][0] = (1.0f - cosA) * xy - axisZ * sinA;
-            mtx->m[1][1] = (1.0f - yy) * cosA + yy;
-            mtx->m[1][2] = (1.0f - cosA) * yz + axisX * sinA;
-            mtx->m[1][3] = 0.0f;
-
-            mtx->m[2][0] = (1.0f - cosA) * xz + axisY * sinA;
-            mtx->m[2][1] = (1.0f - cosA) * yz - axisX * sinA;
-            mtx->m[2][2] = (1.0f - zz) * cosA + zz;
-            mtx->m[2][3] = 0.0f;
-
-            mtx->m[3][0] = mtx->m[3][1] = mtx->m[3][2] = 0.0f;
-            mtx->m[3][3] = 1.0f;
+            cmf->xx = 1.0f;
+            cmf->yx = 0.0f;
+            cmf->zx = 0.0f;
+            cmf->wx = 0.0f;
+            cmf->xy = 0.0f;
+            cmf->yy = 1.0f;
+            cmf->zy = 0.0f;
+            cmf->wy = 0.0f;
+            cmf->xz = 0.0f;
+            cmf->yz = 0.0f;
+            cmf->zz = 1.0f;
+            cmf->wz = 0.0f;
+            cmf->xw = 0.0f;
+            cmf->yw = 0.0f;
+            cmf->zw = 0.0f;
+            cmf->ww = 1.0f;
         }
     }
 }
 
 // Converts the current Gfx matrix to a Mtx
 void Matrix_ToMtx(Mtx* dest) {
-    s32 intVal;
-    u16(*iPart)[4] = dest->intPart;
-    u16(*fPart)[4] = dest->fracPart;
-    Matrix* src = gGfxMatrix;
-
-    intVal = src->m[0][0] * 0x10000;
-    iPart[0][0] = intVal >> 0x10;
-    fPart[0][0] = intVal % 0x10000U;
-
-    intVal = src->m[0][1] * 0x10000;
-    iPart[0][1] = intVal >> 0x10;
-    fPart[0][1] = intVal % 0x10000U;
-
-    intVal = src->m[0][2] * 0x10000;
-    iPart[0][2] = intVal >> 0x10;
-    fPart[0][2] = intVal % 0x10000U;
-
-    intVal = src->m[0][3] * 0x10000;
-    iPart[0][3] = intVal >> 0x10;
-    fPart[0][3] = intVal % 0x10000U;
-
-    intVal = src->m[1][0] * 0x10000;
-    iPart[1][0] = intVal >> 0x10;
-    fPart[1][0] = intVal % 0x10000U;
-
-    intVal = src->m[1][1] * 0x10000;
-    iPart[1][1] = intVal >> 0x10;
-    fPart[1][1] = intVal % 0x10000U;
-
-    intVal = src->m[1][2] * 0x10000;
-    iPart[1][2] = intVal >> 0x10;
-    fPart[1][2] = intVal % 0x10000U;
-
-    intVal = src->m[1][3] * 0x10000;
-    iPart[1][3] = intVal >> 0x10;
-    fPart[1][3] = intVal % 0x10000U;
-
-    intVal = src->m[2][0] * 0x10000;
-    iPart[2][0] = intVal >> 0x10;
-    fPart[2][0] = intVal % 0x10000U;
-
-    intVal = src->m[2][1] * 0x10000;
-    iPart[2][1] = intVal >> 0x10;
-    fPart[2][1] = intVal % 0x10000U;
-
-    intVal = src->m[2][2] * 0x10000;
-    iPart[2][2] = intVal >> 0x10;
-    fPart[2][2] = intVal % 0x10000U;
-
-    intVal = src->m[2][3] * 0x10000;
-    iPart[2][3] = intVal >> 0x10;
-    fPart[2][3] = intVal % 0x10000U;
-
-    intVal = src->m[3][0] * 0x10000;
-    iPart[3][0] = intVal >> 0x10;
-    fPart[3][0] = intVal % 0x10000U;
-
-    intVal = src->m[3][1] * 0x10000;
-    iPart[3][1] = intVal >> 0x10;
-    fPart[3][1] = intVal % 0x10000U;
-
-    intVal = src->m[3][2] * 0x10000;
-    iPart[3][2] = intVal >> 0x10;
-    fPart[3][2] = intVal % 0x10000U;
-
-    intVal = src->m[3][3] * 0x10000;
-    iPart[3][3] = intVal >> 0x10;
-    fPart[3][3] = intVal % 0x10000U;
+    // LTODO: We need to validate this
+    guMtxF2L(gGfxMatrix->m, dest);
 }
 
 // Converts the Mtx src to a Matrix, putting the result in dest
 void Matrix_FromMtx(Mtx* src, Matrix* dest) {
-    dest->m[0][0] = ((src->intPart[0][0] << 0x10) | src->fracPart[0][0]) * (1.0f / 0x10000);
-    dest->m[0][1] = ((src->intPart[0][1] << 0x10) | src->fracPart[0][1]) * (1.0f / 0x10000);
-    dest->m[0][2] = ((src->intPart[0][2] << 0x10) | src->fracPart[0][2]) * (1.0f / 0x10000);
-    dest->m[0][3] = ((src->intPart[0][3] << 0x10) | src->fracPart[0][3]) * (1.0f / 0x10000);
-    dest->m[1][0] = ((src->intPart[1][0] << 0x10) | src->fracPart[1][0]) * (1.0f / 0x10000);
-    dest->m[1][1] = ((src->intPart[1][1] << 0x10) | src->fracPart[1][1]) * (1.0f / 0x10000);
-    dest->m[1][2] = ((src->intPart[1][2] << 0x10) | src->fracPart[1][2]) * (1.0f / 0x10000);
-    dest->m[1][3] = ((src->intPart[1][3] << 0x10) | src->fracPart[1][3]) * (1.0f / 0x10000);
-    dest->m[2][0] = ((src->intPart[2][0] << 0x10) | src->fracPart[2][0]) * (1.0f / 0x10000);
-    dest->m[2][1] = ((src->intPart[2][1] << 0x10) | src->fracPart[2][1]) * (1.0f / 0x10000);
-    dest->m[2][2] = ((src->intPart[2][2] << 0x10) | src->fracPart[2][2]) * (1.0f / 0x10000);
-    dest->m[2][3] = ((src->intPart[2][3] << 0x10) | src->fracPart[2][3]) * (1.0f / 0x10000);
-    dest->m[3][0] = ((src->intPart[3][0] << 0x10) | src->fracPart[3][0]) * (1.0f / 0x10000);
-    dest->m[3][1] = ((src->intPart[3][1] << 0x10) | src->fracPart[3][1]) * (1.0f / 0x10000);
-    dest->m[3][2] = ((src->intPart[3][2] << 0x10) | src->fracPart[3][2]) * (1.0f / 0x10000);
-    dest->m[3][3] = ((src->intPart[3][3] << 0x10) | src->fracPart[3][3]) * (1.0f / 0x10000);
+    guMtxF2L(src->m, dest->m);
 }
 
 // Applies the transform matrix mtx to the vector src, putting the result in dest
diff --git a/src/sys/sys_save.c b/src/sys/sys_save.c
index 13cf52ea..17afd829 100644
--- a/src/sys/sys_save.c
+++ b/src/sys/sys_save.c
@@ -19,7 +19,7 @@ s32 Save_WriteBlock(s32 arg0, u8* arg1) {
         (void) "ＥＥＰＲＯＭ インターフェース回路反応なし (ＷＲＩＴＥ)\n";
         return -1;
     }
-    Timer_Wait(MSEC_TO_CYCLES(15));
+    // Timer_Wait(MSEC_TO_CYCLES(15));
     (void) "EEPROM WRITE %02X: %02X %02X %02X %02X %02X %02X %02X %02X\n";
     return 0;
 }
diff --git a/src/sys/sys_timer.c b/src/sys/sys_timer.c
index 7b11b71d..f84577fe 100644
--- a/src/sys/sys_timer.c
+++ b/src/sys/sys_timer.c
@@ -13,7 +13,7 @@ TimerTask* Timer_AllocateTask(void) {
     return NULL;
 }
 
-s32 Timer_CreateTask(u64 time, TimerAction action, s32* address, s32 value) {
+int32_t Timer_CreateTask(uint64_t time, TimerAction action, s32* address, s32 value) {
     TimerTask* task = Timer_AllocateTask();
 
     if (task == NULL) {
@@ -41,10 +41,4 @@ void Timer_CompleteTask(TimerTask* task) {
     task->active = false;
 }
 
-void Timer_Wait(u64 time) {
-    OSTimer timer;
-    OSMesg dummy;
-
-    osSetTimer(&timer, time, 0, &gTimerWaitMsgQueue, OS_MESG_PTR(NULL));
-    osRecvMesg(&gTimerWaitMsgQueue, &dummy, OS_MESG_BLOCK);
-}
+void
\ No newline at end of file
diff --git a/tools/Torch b/tools/Torch
index ad667b70..4d654e65 160000
--- a/tools/Torch
+++ b/tools/Torch
@@ -1 +1 @@
-Subproject commit ad667b705dad3476a1ae803f23588c55baab2ee4
+Subproject commit 4d654e65a9c04b7b5486f792cf5f495186dd9311