From e22f8ea6a853bdf2b19c5c24cf34c1caf71e2056 Mon Sep 17 00:00:00 2001
From: Kermalis <29823718+Kermalis@users.noreply.github.com>
Date: Tue, 5 Nov 2024 08:26:09 -0500
Subject: [PATCH] sub_800A5A4

---
 asm/math.s            | 160 ------------------------------------------
 include/number_util.h |   2 +-
 src/math.c            | 150 +++++++++++++++++++++++++++++----------
 3 files changed, 113 insertions(+), 199 deletions(-)

diff --git a/asm/math.s b/asm/math.s
index 752203551..d6a14e917 100644
--- a/asm/math.s
+++ b/asm/math.s
@@ -5,166 +5,6 @@
 
 	.text
 
-	thumb_func_start sub_800A5A4
-sub_800A5A4:
-	push {r4-r7,lr}
-	mov r7, r10
-	mov r6, r9
-	mov r5, r8
-	push {r5-r7}
-	sub sp, 0x14
-	str r0, [sp]
-	adds r5, r1, 0
-	mov r8, r2
-	mov r0, r8
-	bl sub_800A2A0
-	lsls r0, 24
-	lsrs r4, r0, 24
-	cmp r4, 0
-	beq _0800A5D8
-	ldr r0, _0800A5D4
-	ldr r1, [sp]
-	str r0, [r1]
-	movs r0, 0x1
-	negs r0, r0
-	str r0, [r1, 0x4]
-	b _0800A6BE
-	.align 2, 0
-_0800A5D4: .4byte 0x7fffffff
-_0800A5D8:
-	adds r0, r5, 0
-	bl sub_800A2A0
-	lsls r0, 24
-	cmp r0, 0
-	beq _0800A5EC
-	ldr r2, [sp]
-	str r4, [r2]
-	str r4, [r2, 0x4]
-	b _0800A6BE
-_0800A5EC:
-	ldr r0, [r5]
-	lsls r7, r0, 16
-	ldr r1, [r5, 0x4]
-	lsrs r0, r1, 16
-	orrs r7, r0
-	lsls r6, r1, 16
-	movs r0, 0x80
-	lsls r0, 8
-	orrs r6, r0
-	mov r0, r8
-	ldr r0, [r0]
-	str r0, [sp, 0x4]
-	mov r1, r8
-	ldr r1, [r1, 0x4]
-	str r1, [sp, 0x8]
-	movs r2, 0
-	str r2, [sp, 0xC]
-	mov r9, r2
-	movs r5, 0
-	movs r4, 0
-	movs r0, 0x80
-	lsls r0, 24
-	mov r10, r0
-	movs r1, 0x1
-	mov r8, r1
-	movs r2, 0x3F
-	str r2, [sp, 0x10]
-_0800A622:
-	lsls r5, 1
-	adds r0, r4, 0
-	mov r1, r10
-	ands r0, r1
-	cmp r0, 0
-	beq _0800A632
-	mov r2, r8
-	orrs r5, r2
-_0800A632:
-	movs r1, 0x2
-	negs r1, r1
-	lsls r4, 1
-	adds r0, r7, 0
-	mov r2, r10
-	ands r0, r2
-	cmp r0, 0
-	beq _0800A646
-	mov r0, r8
-	orrs r4, r0
-_0800A646:
-	lsls r7, 1
-	adds r0, r6, 0
-	mov r2, r10
-	ands r0, r2
-	cmp r0, 0
-	beq _0800A656
-	mov r0, r8
-	orrs r7, r0
-_0800A656:
-	lsls r6, 1
-	ands r6, r1
-	adds r0, r5, 0
-	adds r1, r4, 0
-	ldr r2, [sp, 0x4]
-	ldr r3, [sp, 0x8]
-	bl u32_pair_less_than
-	lsls r0, 24
-	cmp r0, 0
-	bne _0800A680
-	adds r0, r4, 0
-	movs r1, 0x1
-	ldr r2, [sp, 0x8]
-	subs r4, r2
-	ldr r2, [sp, 0x4]
-	subs r5, r2
-	cmp r0, r4
-	bcs _0800A682
-	subs r5, 0x1
-	b _0800A682
-_0800A680:
-	movs r1, 0
-_0800A682:
-	ldr r0, [sp, 0xC]
-	lsls r0, 1
-	str r0, [sp, 0xC]
-	mov r0, r9
-	mov r2, r10
-	ands r0, r2
-	cmp r0, 0
-	beq _0800A69A
-	ldr r0, [sp, 0xC]
-	mov r2, r8
-	orrs r0, r2
-	str r0, [sp, 0xC]
-_0800A69A:
-	mov r0, r9
-	lsls r0, 1
-	mov r9, r0
-	cmp r1, 0
-	beq _0800A6AA
-	mov r1, r8
-	orrs r0, r1
-	mov r9, r0
-_0800A6AA:
-	ldr r2, [sp, 0x10]
-	subs r2, 0x1
-	str r2, [sp, 0x10]
-	cmp r2, 0
-	bge _0800A622
-	ldr r0, [sp, 0xC]
-	ldr r1, [sp]
-	str r0, [r1]
-	mov r2, r9
-	str r2, [r1, 0x4]
-_0800A6BE:
-	add sp, 0x14
-	pop {r3-r5}
-	mov r8, r3
-	mov r9, r4
-	mov r10, r5
-	pop {r4-r7}
-	pop {r0}
-	bx r0
-	thumb_func_end sub_800A5A4
-
 	thumb_func_start sub_800A6D0
 sub_800A6D0:
 	push {r4,lr}
diff --git a/include/number_util.h b/include/number_util.h
index 484cfda5c..8f85eedef 100644
--- a/include/number_util.h
+++ b/include/number_util.h
@@ -10,7 +10,7 @@ typedef struct FixedPoint
 typedef struct unkStruct_80943A8
 {
     s32 s0;
-    s32 s4;
+    u32 s4; // sub_800A5A4 requires this to be a u32, but other funcs check if it's negative...
 } unkStruct_80943A8;
 
 FixedPoint FixedPoint_Add(FixedPoint a, FixedPoint b);
diff --git a/src/math.c b/src/math.c
index b1cb1b217..6194c4294 100644
--- a/src/math.c
+++ b/src/math.c
@@ -4,11 +4,11 @@
 
 #include "data/math.h"
 
-u24_8 u24_8_div(u24_8, u24_8);
-u24_8 u24_8_mul(u24_8, u24_8);
-bool8 u32_pair_less_than(u32, u32, u32, u32);
+static u24_8 u24_8_div(u24_8, u24_8);
+static u24_8 u24_8_mul(u24_8, u24_8);
+static bool8 u32_pair_less_than(u32, u32, u32, u32);
 
-void sub_800A5A4(unkStruct_80943A8 *, unkStruct_80943A8 *, unkStruct_80943A8 *);
+static void sub_800A5A4(unkStruct_80943A8 *, unkStruct_80943A8 *, unkStruct_80943A8 *);
 static void sub_800A4E4(unkStruct_80943A8 *, unkStruct_80943A8 *, unkStruct_80943A8 *);
 
 /**
@@ -75,7 +75,7 @@ s32 cos_4096(s32 x)
  *
  * @return          `TRUE` if `x < y`, `FALSE` otherwise.
  */
-bool8 u32_pair_less_than(u32 x_hi, u32 x_lo, u32 y_hi, u32 y_lo)
+static bool8 u32_pair_less_than(u32 x_hi, u32 x_lo, u32 y_hi, u32 y_lo)
 {
     if (x_hi < y_hi)
         return TRUE;
@@ -154,7 +154,7 @@ s24_8 s24_8_div(s24_8 x, s24_8 y)
  *
  * @return        The product `x*y` as an unsigned 24.8 fixed-point number.
  */
-u24_8 u24_8_mul(u24_8 x, u24_8 y)
+static u24_8 u24_8_mul(u24_8 x, u24_8 y)
 {
     // We need 64 bits for intermediate steps of the multiplication.
     u32 x_h, x_l;
@@ -220,7 +220,7 @@ u24_8 u24_8_mul(u24_8 x, u24_8 y)
  *
  * @return        The quotient `x/y` as an unsigned 24.8 fixed-point number.
  */
-u24_8 u24_8_div(u24_8 x, u24_8 y)
+static u24_8 u24_8_div(u24_8 x, u24_8 y)
 {
   bool8 bVar1;
   u32 r9;
@@ -351,39 +351,40 @@ s32 sub_8009FB8(s32 x, s32 y)
 void sub_800A020(unkStruct_80943A8 *param_1, u32 param_2)
 {
 #ifndef NONMATCHING
-  register u32 temp asm("r4");
+    register u32 temp asm("r4");
 #else
-  u32 temp;
+    u32 temp;
 #endif
-    
-  temp = 0xffff0000;
-  param_1->s0 = param_2 >> 0x10;
-  param_1->s4 = param_2 << 0x10;
-  if ((param_2 & 0x8000) != 0) {
-    param_1->s0 |= temp;
-  }
+
+    temp = 0xFFFF0000;
+    param_1->s0 = param_2 >> 16;
+    param_1->s4 = param_2 << 16;
+
+    if (param_2 & 0x8000)
+        param_1->s0 |= temp;
+
 }
 
-u32 sub_800A048(u32 *param_1)
+u32 sub_800A048(u32 *a)
 {
-  u32 uVar1;
-  
-  uVar1 = ((u16)param_1[0] << 0x10) | (param_1[1] >> 0x10);
-  if ((param_1[1] & 0x8000) != 0) {
-    uVar1++;
-  }
-  return uVar1;
+    u32 uVar1;
+
+    uVar1 = ((u16)a[0] << 16) | (a[1] >> 16);
+    if (a[1] & 0x8000)
+        uVar1++;
+
+    return uVar1;
 }
 
-UNUSED u32 sub_800A068(u32 *param_1)
+UNUSED u32 sub_800A068(u32 *a)
 {
-  u32 uVar1;
-  
-  uVar1 = ((u8)param_1[0] << 0x18) | param_1[1] >> 8;
-  if ((param_1[1] & 0x8000) != 0) {
-    uVar1++;
-  }
-  return uVar1;
+    u32 uVar1;
+
+    uVar1 = ((u8)a[0] << 24) | a[1] >> 8;
+    if (a[1] & 0x8000)
+        uVar1++;
+
+    return uVar1;
 }
 
 void sub_800A088(u32 *a, s32 b)
@@ -518,9 +519,9 @@ static void sub_800A25C(unkStruct_80943A8 *a)
 {
     a->s0 = ~a->s0;
     a->s4 = ~a->s4 + 1;
-    if (a->s4 == 0) {
+
+    if (a->s4 == 0)
         a->s0++;
-    }
 }
 
 void sub_800A27C(unkStruct_80943A8 *a)
@@ -528,9 +529,9 @@ void sub_800A27C(unkStruct_80943A8 *a)
     if (a->s0 < 0) {
         a->s0 = ~a->s0;
         a->s4 = ~a->s4 + 1;
-        if (a->s4 == 0) {
+
+        if (a->s4 == 0)
             a->s0++;
-        }
     }
 }
 
@@ -566,7 +567,7 @@ bool8 sub_800A2F0(unkStruct_80943A8 *a, unkStruct_80943A8 *b)
 
     b0 = b->s0;
     if (b0 < 0)
-        r1 |= 2;
+        r1 |= 0x2;
 
     switch (r1) {
         case 0:
@@ -637,7 +638,7 @@ void sub_800A3F0(unkStruct_80943A8 *dst, unkStruct_80943A8 *a, unkStruct_80943A8
 
     if (sub_800A2A0(&bb)) {
         dst->s0 = INT32_MAX;
-        dst->s4 = -1;
+        dst->s4 = UINT32_MAX;
     }
     else if (sub_800A2A0(&aa)) {
         dst->s0 = 0;
@@ -747,4 +748,77 @@ static void sub_800A4E4(unkStruct_80943A8 *dst, unkStruct_80943A8 *a, unkStruct_
         dst->s0 = r6;
         dst->s4 = r5;
     }
+}
+
+static void sub_800A5A4(unkStruct_80943A8 *dst, unkStruct_80943A8 *a, unkStruct_80943A8 *b)
+{
+    s32 temp;
+    bool8 r1;
+    u32 r4;
+    u32 r5;
+    u32 r6;
+    u32 r7;
+    u32 r9;
+    s32 sp4;
+    u32 sp8;
+    u32 spC;
+    s32 i;
+
+    if (sub_800A2A0(b)) {
+        dst->s0 = INT32_MAX;
+        dst->s4 = UINT32_MAX;
+    }
+    else if (sub_800A2A0(a)) {
+        dst->s0 = 0;
+        dst->s4 = 0;
+    }
+    else {
+        r7 = (a->s0 << 16) | (a->s4 >> 16);
+        r6 = (a->s4 << 16) | 0x8000;
+        sp4 = b->s0;
+        sp8 = b->s4;
+        spC = 0; // Effectively unused
+        r9 = 0;
+        r5 = 0;
+        r4 = 0;
+
+        for (i = 0; i < 64; i++) {
+            r5 <<= 1;
+            if (r4 & 0x80000000)
+                r5 |= 0x1;
+
+            r4 = (r4 << 1) & ~0x1;
+            if (r7 & 0x80000000)
+                r4 |= 0x1;
+
+            r7 <<= 1;
+            if (r6 & 0x80000000)
+                r7 |= 0x1;
+
+            r6 = (r6 << 1) & ~0x1;
+
+            if (!u32_pair_less_than(r5, r4, sp4, sp8)) {
+                temp = r4;
+                r1 = TRUE;
+                r4 -= sp8;
+                r5 -= sp4;
+
+                if (temp < r4)
+                    r5--;
+            }
+            else
+                r1 = FALSE;
+
+            spC <<= 1;
+            if (r9 & 0x80000000)
+                spC |= 0x1;
+
+            r9 = (r9 << 1) & ~0x1;
+            if (r1)
+                r9 |= 0x1;
+        }
+
+        dst->s0 = spC;
+        dst->s4 = r9;
+    }
 }
\ No newline at end of file