From 2d20f33494927e59dca1eeb662622be28fbb7e9d Mon Sep 17 00:00:00 2001
From: Elmyr de Hory <85423474+Elmyr-de-Hory@users.noreply.github.com>
Date: Sun, 23 Apr 2023 12:33:09 -0700
Subject: [PATCH] matched sub_08068318

---
 .../zeldaFollower/sub_08068318.inc            | 287 ------------------
 src/npc/zeldaFollower.c                       | 141 ++++++++-
 2 files changed, 125 insertions(+), 303 deletions(-)
 delete mode 100644 asm/non_matching/zeldaFollower/sub_08068318.inc

diff --git a/asm/non_matching/zeldaFollower/sub_08068318.inc b/asm/non_matching/zeldaFollower/sub_08068318.inc
deleted file mode 100644
index 620448a5..00000000
--- a/asm/non_matching/zeldaFollower/sub_08068318.inc
+++ /dev/null
@@ -1,287 +0,0 @@
-	.syntax unified
-	push {r4, r5, r6, r7, lr}
-	mov r7, sb
-	mov r6, r8
-	push {r6, r7}
-	mov r8, r0
-	ldr r4, _08068414 @ =gPlayerEntity
-	ldrh r1, [r4, #0x2e]
-	ldr r3, _08068418 @ =0xFFFF0000
-	adds r0, r3, #0
-	ands r0, r6
-	orrs r0, r1
-	ldrh r1, [r4, #0x32]
-	lsls r1, r1, #0x10
-	ldr r2, _0806841C @ =0x0000FFFF
-	ands r0, r2
-	adds r6, r0, #0
-	orrs r6, r1
-	ldrh r0, [r4, #0x36]
-	ands r3, r7
-	orrs r3, r0
-	adds r7, r3, #0
-	ldr r0, _08068420 @ =gPlayerState
-	adds r0, #0xa8
-	ldrb r0, [r0]
-	lsls r0, r0, #0x10
-	ldr r2, _08068424 @ =0xFF00FFFF
-	ands r2, r3
-	orrs r2, r0
-	adds r7, r2, #0
-	ldrb r1, [r4, #0x14]
-	movs r0, #0x3f
-	ands r1, r0
-	lsls r1, r1, #0x18
-	ldr r0, _08068428 @ =0xC0FFFFFF
-	ands r0, r2
-	orrs r0, r1
-	adds r7, r0, #0
-	adds r1, r4, #0
-	adds r1, #0x38
-	ldrb r1, [r1]
-	lsls r1, r1, #0x1e
-	ldr r2, _0806842C @ =0x3FFFFFFF
-	ands r0, r2
-	adds r7, r0, #0
-	orrs r7, r1
-	mov r0, r8
-	ldr r5, [r0, #0x64]
-	ldrb r1, [r5, #6]
-	cmp r1, #0xa
-	bne _08068386
-	lsrs r0, r7, #0x10
-	lsls r0, r0, #0x18
-	lsrs r0, r0, #0x18
-	cmp r0, #0xa
-	bne _08068394
-_08068386:
-	cmp r1, #0x16
-	bne _080683B4
-	lsrs r0, r7, #0x10
-	lsls r0, r0, #0x18
-	lsrs r0, r0, #0x18
-	cmp r0, #0x16
-	beq _080683B4
-_08068394:
-	ldrh r0, [r4, #0x2e]
-	mov r1, r8
-	strh r0, [r1, #0x2e]
-	ldrh r0, [r4, #0x32]
-	strh r0, [r1, #0x32]
-	ldrb r1, [r1, #0x18]
-	movs r0, #4
-	rsbs r0, r0, #0
-	ands r0, r1
-	movs r1, #1
-	orrs r0, r1
-	mov r2, r8
-	strb r0, [r2, #0x18]
-	mov r0, r8
-	bl sub_08068578
-_080683B4:
-	movs r3, #0
-	mov sb, r3
-	ldr r0, [r5]
-	cmp r6, r0
-	bne _080683D0
-	ldr r0, [r5, #4]
-	cmp r7, r0
-	bne _080683D0
-	lsls r1, r7, #8
-	lsrs r0, r1, #0x18
-	cmp r0, #0x16
-	beq _080683D0
-	cmp r0, #0xa
-	bne _08068430
-_080683D0:
-	mov r0, r8
-	ldr r5, [r0, #0x64]
-	adds r5, #0x90
-	mov r4, r8
-	adds r4, #0x69
-	mov r3, r8
-	adds r3, #0x38
-	movs r1, #0x58
-	add r1, r8
-	mov ip, r1
-	movs r2, #0x12
-_080683E6:
-	ldr r0, [r5]
-	ldr r1, [r5, #4]
-	str r0, [r5, #8]
-	str r1, [r5, #0xc]
-	subs r5, #8
-	subs r2, #1
-	cmp r2, #0
-	bge _080683E6
-	mov r2, r8
-	ldr r5, [r2, #0x64]
-	str r6, [r5]
-	str r7, [r5, #4]
-	movs r0, #4
-	mov sb, r0
-	ldrb r1, [r4]
-	movs r0, #0
-	ldrsb r0, [r4, r0]
-	cmp r0, #0
-	ble _080684A8
-	subs r0, r1, #1
-	strb r0, [r4]
-	b _080684A8
-	.align 2, 0
-_08068414: .4byte gPlayerEntity
-_08068418: .4byte 0xFFFF0000
-_0806841C: .4byte 0x0000FFFF
-_08068420: .4byte gPlayerState
-_08068424: .4byte 0xFF00FFFF
-_08068428: .4byte 0xC0FFFFFF
-_0806842C: .4byte 0x3FFFFFFF
-_08068430:
-	adds r5, #0x98
-	movs r1, #4
-	ldrsh r0, [r5, r1]
-	cmp r0, #0
-	bge _08068466
-	mov r2, r8
-	ldr r5, [r2, #0x64]
-	adds r5, #0x90
-	mov r4, r8
-	adds r4, #0x69
-	mov r3, r8
-	adds r3, #0x38
-	movs r0, #0x58
-	add r0, r8
-	mov ip, r0
-	movs r2, #0x12
-_08068450:
-	ldr r0, [r5]
-	ldr r1, [r5, #4]
-	str r0, [r5, #8]
-	str r1, [r5, #0xc]
-	subs r5, #8
-	subs r2, #1
-	cmp r2, #0
-	bge _08068450
-	movs r1, #4
-	mov sb, r1
-	b _080684A8
-_08068466:
-	ldr r1, _0806851C @ =gPlayerEntity
-	movs r2, #0x2e
-	ldrsh r0, [r1, r2]
-	movs r3, #0x32
-	ldrsh r1, [r1, r3]
-	ldrh r2, [r5]
-	ldrh r3, [r5, #2]
-	bl sub_080041E8
-	lsrs r0, r0, #4
-	mov r4, r8
-	adds r4, #0x69
-	mov r3, r8
-	adds r3, #0x38
-	movs r1, #0x58
-	add r1, r8
-	mov ip, r1
-	cmp r0, #0x18
-	ble _080684A8
-	mov r2, r8
-	ldr r5, [r2, #0x64]
-	adds r5, #0x90
-	movs r2, #0x12
-_08068494:
-	ldr r0, [r5]
-	ldr r1, [r5, #4]
-	str r0, [r5, #8]
-	str r1, [r5, #0xc]
-	subs r5, #8
-	subs r2, #1
-	cmp r2, #0
-	bge _08068494
-	movs r0, #4
-	mov sb, r0
-_080684A8:
-	mov r1, r8
-	ldr r5, [r1, #0x64]
-	adds r5, #0x98
-	ldrh r0, [r5]
-	strh r0, [r1, #0x2e]
-	ldrh r0, [r5, #2]
-	strh r0, [r1, #0x32]
-	ldrh r0, [r5, #4]
-	strh r0, [r1, #0x36]
-	ldrb r0, [r5, #7]
-	lsls r0, r0, #0x1a
-	lsrs r0, r0, #0x1a
-	strb r0, [r1, #0x14]
-	ldrb r0, [r5, #7]
-	lsrs r0, r0, #6
-	strb r0, [r3]
-	ldrb r0, [r5, #6]
-	cmp r0, #0x16
-	beq _080684D2
-	cmp r0, #0xa
-	bne _080684DE
-_080684D2:
-	mov r2, r8
-	ldrb r1, [r2, #0x18]
-	movs r0, #4
-	rsbs r0, r0, #0
-	ands r0, r1
-	strb r0, [r2, #0x18]
-_080684DE:
-	adds r1, r4, #0
-	ldrb r2, [r1]
-	movs r0, #0
-	ldrsb r0, [r1, r0]
-	cmp r0, #0
-	ble _080684EE
-	subs r0, r2, #1
-	strb r0, [r1]
-_080684EE:
-	mov r2, sb
-	mov r3, r8
-	ldrb r0, [r3, #0x14]
-	lsrs r0, r0, #1
-	add sb, r0
-	mov r0, ip
-	ldrb r0, [r0]
-	cmp sb, r0
-	beq _08068520
-	cmp r2, #0
-	bne _0806850C
-	movs r0, #0
-	ldrsb r0, [r1, r0]
-	cmp r0, #0
-	bgt _08068520
-_0806850C:
-	mov r0, r8
-	mov r1, sb
-	bl InitAnimationForceUpdate
-	movs r0, #0x1e
-	strb r0, [r4]
-	b _08068526
-	.align 2, 0
-_0806851C: .4byte gPlayerEntity
-_08068520:
-	mov r0, r8
-	bl UpdateAnimationSingleFrame
-_08068526:
-	mov r0, r8
-	bl sub_0800451C
-	mov r1, r8
-	movs r2, #0x36
-	ldrsh r0, [r1, r2]
-	cmp r0, #0
-	bge _08068542
-	movs r2, #0xc
-	rsbs r2, r2, #0
-	mov r0, r8
-	movs r1, #0
-	bl sub_0806F854
-_08068542:
-	pop {r3, r4}
-	mov r8, r3
-	mov sb, r4
-	pop {r4, r5, r6, r7, pc}
-	.align 2, 0
-	.syntax divided
diff --git a/src/npc/zeldaFollower.c b/src/npc/zeldaFollower.c
index ae67fcba..24d9c30c 100644
--- a/src/npc/zeldaFollower.c
+++ b/src/npc/zeldaFollower.c
@@ -3,16 +3,36 @@
 #include "functions.h"
 #include "common.h"
 
-typedef struct {
-    u16 x;
-    u16 y;
-    u16 z;
-    u8 framestate;
-    u8 animationState : 6;
-    u8 collisionLayer : 2;
+extern s32 sub_080041E8(s32 x1, s32 y1, s32 x2, s32 y2);
+
+typedef union {
+    struct {
+        u16 x;
+        u16 y;
+        u16 z;
+        u8 framestate;
+        u8 animationState : 6;
+        u8 collisionLayer : 2;
+    } FIELDS;
+    u64 DWORD;
 } ZeldaFollowerItem;
 
-typedef ZeldaFollowerItem ZeldaFollowerHeap[20];
+#define ZELDA_FOLLOWER_HEAP_LEN 20
+
+#define ZELDA_FOLLOWER_HEAP ((ZeldaFollowerItem *)this->myHeap)
+#define ZELDA_FOLLOWER_HEAP_END ((ZeldaFollowerItem *)this->myHeap + (ZELDA_FOLLOWER_HEAP_LEN - 1))
+
+#define ZELDA_FOLLOWER_HEAP_SHIFT_RIGHT(this, heapPtr)                \
+        do {                                                          \
+            int i;                                                    \
+            heapPtr = ((ZeldaFollowerItem*)this->myHeap);             \
+            heapPtr += (ZELDA_FOLLOWER_HEAP_LEN - 2);                 \
+            for ( i = 0; i < (ZELDA_FOLLOWER_HEAP_LEN - 1); i++) {    \
+                heapPtr[1] = heapPtr[0];                              \
+                heapPtr--;                                            \
+            }                                                         \
+        } while (0)
+
 
 void sub_08068318(Entity*);
 void sub_0806854C(Entity*, u32*);
@@ -36,10 +56,99 @@ void ZeldaFollower(Entity* this) {
     }
 }
 
-ASM_FUNC("asm/non_matching/zeldaFollower/sub_08068318.inc", void sub_08068318(Entity* this))
+void sub_08068318(Entity* this) {
+    s32 dist;
+    s16 z;
+
+    u32 animIndex;
+    u32 animIndexTmp;
+
+    ZeldaFollowerItem* heapPtr;
+    ZeldaFollowerItem item;
+
+    item.FIELDS.x              = gPlayerEntity.x.HALF_U.HI;
+    item.FIELDS.y              = gPlayerEntity.y.HALF_U.HI;
+    item.FIELDS.z              = gPlayerEntity.z.HALF_U.HI;
+    item.FIELDS.framestate     = gPlayerState.framestate;
+    item.FIELDS.animationState = gPlayerEntity.animationState;
+    item.FIELDS.collisionLayer = gPlayerEntity.collisionLayer;
+
+    heapPtr = this->myHeap;
+
+    if ( (heapPtr->FIELDS.framestate == 0xa  && item.FIELDS.framestate != 0xa) ||
+        (heapPtr->FIELDS.framestate == 0x16 && item.FIELDS.framestate != 0x16)) {
+        this->x.HALF.HI = gPlayerEntity.x.HALF.HI;
+        this->y.HALF.HI = gPlayerEntity.y.HALF.HI;
+        this->spriteSettings.draw = 1;
+        sub_08068578(this);
+    }
+
+    animIndex = 0;
+    if (item.DWORD != heapPtr->DWORD || item.FIELDS.framestate == 0x16 || item.FIELDS.framestate == 0xa ) {
+        ZELDA_FOLLOWER_HEAP_SHIFT_RIGHT(this, heapPtr);
+        heapPtr = ZELDA_FOLLOWER_HEAP;
+        heapPtr[0] = item;
+        animIndex = 0x4;
+
+        if ((s8)this->field_0x68.HALF.HI > 0) {
+            this->field_0x68.HALF.HI = this->field_0x68.HALF.HI - 1;
+        }
+    } else {
+        heapPtr += ZELDA_FOLLOWER_HEAP_LEN - 1;
+        z = heapPtr->FIELDS.z;
+
+        if (z < 0) {
+            ZELDA_FOLLOWER_HEAP_SHIFT_RIGHT(this, heapPtr);
+            animIndex = 0x4;
+        } else {
+            dist = sub_080041E8(
+                gPlayerEntity.x.HALF.HI,
+                gPlayerEntity.y.HALF.HI,
+                (u16) heapPtr->FIELDS.x,
+                (u16) heapPtr->FIELDS.y
+            );
+            dist = ((u32)dist) >> 0x4;
+            if (dist > 0x18) {
+                ZELDA_FOLLOWER_HEAP_SHIFT_RIGHT(this, heapPtr);
+                animIndex = 0x4;
+            }
+        }
+    }
+
+    heapPtr = ZELDA_FOLLOWER_HEAP;
+    heapPtr += ZELDA_FOLLOWER_HEAP_LEN - 1;
+    this->x.HALF.HI      = heapPtr->FIELDS.x;
+    this->y.HALF.HI      = heapPtr->FIELDS.y;
+    this->z.HALF.HI      = heapPtr->FIELDS.z;
+    this->animationState = heapPtr->FIELDS.animationState;
+    this->collisionLayer = heapPtr->FIELDS.collisionLayer;
+
+    if (heapPtr->FIELDS.framestate == 0x16 || heapPtr->FIELDS.framestate == 0xa) {
+        this->spriteSettings.draw = 0;
+    }
+
+    if (((s8)this->field_0x68.HALF.HI) > 0) {
+        this->field_0x68.HALF.HI = this->field_0x68.HALF.HI - 1;
+    }
+
+    animIndexTmp = animIndex;
+    animIndex += this->animationState >> 1;
+
+    if (!(animIndex == this->animIndex || (animIndexTmp == 0 && ((s8)this->field_0x68.HALF.HI) > 0))) {
+        InitAnimationForceUpdate(this, animIndex);
+        this->field_0x68.HALF.HI = 0x1e;
+    } else {
+        UpdateAnimationSingleFrame(this);
+    }
+
+    sub_0800451C(this);
+    if (this->z.HALF.HI < 0) {
+        sub_0806F854(this, 0x0, -0xc);
+    }
+}
 
 void sub_0806854C(Entity* this, u32* none) {
-    this->myHeap = zMalloc(sizeof(ZeldaFollowerHeap));
+    this->myHeap = zMalloc(sizeof(ZeldaFollowerItem[ZELDA_FOLLOWER_HEAP_LEN]));
     if (this->myHeap != NULL) {
         this->field_0x68.HALF.LO = 1;
         sub_080788E0(this);
@@ -157,12 +266,12 @@ void sub_08068578(Entity* this) {
     // Down here the u32 are suddendly accessed correctly as u16 and bitfields?
     // How are the results of above u32 calculations used?
     for (index = 0x13; index >= 0; index--) {
-        item->x = r5 - (x >> 8);
-        item->y = (r5 >> 0x10) - (y >> 8);
-        item->z = r6;
-        item->framestate = r6 >> 0x10;
-        item->animationState = this->animationState & 0x3f;
-        item->collisionLayer = this->collisionLayer;
+        item->FIELDS.x = r5 - (x >> 8);
+        item->FIELDS.y = (r5 >> 0x10) - (y >> 8);
+        item->FIELDS.z = r6;
+        item->FIELDS.framestate = r6 >> 0x10;
+        item->FIELDS.animationState = this->animationState & 0x3f;
+        item->FIELDS.collisionLayer = this->collisionLayer;
         item++;
         y = y + r8;
         x = x + r10;