diff --git a/src/engine/math_util.c b/src/engine/math_util.c
index d12b19361..61ab9ca05 100644
--- a/src/engine/math_util.c
+++ b/src/engine/math_util.c
@@ -134,8 +134,15 @@ void *vec3f_cross(Vec3f dest, Vec3f a, Vec3f b) {
 
 /// Scale vector 'dest' so it has length 1
 void *vec3f_normalize(Vec3f dest) {
-    //! Possible division by zero
-    f32 invsqrt = 1.0f / sqrtf(dest[0] * dest[0] + dest[1] * dest[1] + dest[2] * dest[2]);
+    f32 div = sqrtf(dest[0] * dest[0] + dest[1] * dest[1] + dest[2] * dest[2]);
+    if (div == 0) {
+        dest[0] = 0;
+        dest[1] = 0;
+        dest[2] = 0;
+        return dest;
+    }
+
+    f32 invsqrt = 1.0f / div;
 
     dest[0] *= invsqrt;
     dest[1] *= invsqrt;
diff --git a/src/engine/surface_collision.c b/src/engine/surface_collision.c
index 4a6fa962b..744e8d7cc 100644
--- a/src/engine/surface_collision.c
+++ b/src/engine/surface_collision.c
@@ -11,6 +11,89 @@
 #include "game/game_init.h"
 #include "pc/utils/misc.h"
 #include "pc/network/network.h"
+#include "src/game/debug.h" // DO NOT COMMIT
+
+Vec3f gFindWallDirection = { 0 };
+u8 gFindWallDirectionActive = false;
+
+#define CLAMP(_val, _min, _max) MAX(MIN((_val), _max), _min)
+
+static void closest_point_to_triangle(struct Surface* surf, Vec3f src, Vec3f out) {
+    Vec3f v1; vec3s_to_vec3f(v1, surf->vertex1);
+    Vec3f v2; vec3s_to_vec3f(v2, surf->vertex2);
+    Vec3f v3; vec3s_to_vec3f(v3, surf->vertex3);
+
+    Vec3f edge0; vec3f_dif(edge0, v2, v1);
+    Vec3f edge1; vec3f_dif(edge1, v3, v1);
+    Vec3f v0;    vec3f_dif(v0,    v1, src);
+
+    f32 a = vec3f_dot(edge0, edge0);
+    f32 b = vec3f_dot(edge0, edge1);
+    f32 c = vec3f_dot(edge1, edge1);
+    f32 d = vec3f_dot(edge0, v0);
+    f32 e = vec3f_dot(edge1, v0);
+
+    f32 det = (a * c) - (b * b);
+    f32 s   = (b * e) - (c * d);
+    f32 t   = (b * d) - (a * e);
+
+    if ((s + t) < det) {
+        if (s < 0) {
+            if (t < 0) {
+                if (d < 0) {
+                    s = CLAMP(-d/a, 0, 1);
+                    t = 0;
+                } else {
+                    s = 0;
+                    t = CLAMP(-e/c, 0, 1);
+                }
+            } else {
+                s = 0;
+                t = CLAMP(-e/c, 0, 1);
+            }
+        } else if (t < 0) {
+            s = CLAMP(-d/a, 0, 1);
+            t = 0;
+        } else {
+            f32 invDet = 1 / det;
+            s *= invDet;
+            t *= invDet;
+        }
+    } else {
+        if (s < 0) {
+            f32 tmp0 = (b + d);
+            f32 tmp1 = (c + e);
+            if (tmp1 > tmp0) {
+                f32 numer = tmp1 - tmp0;
+                f32 denom = a-2*b+c;
+                s = CLAMP(numer/denom, 0, 1);
+                t = (1 - s);
+            } else {
+                t = CLAMP(-e/c, 0, 1);
+                s = 0;
+            }
+        } else if (t < 0.f) {
+            if ((a + d) > (b + e)) {
+                f32 numer = c+e-b-d;
+                f32 denom = a-2*b+c;
+                s = CLAMP(numer/denom, 0, 1);
+                t = (1 - s);
+            } else {
+                s = CLAMP(-e/c, 0, 1);
+                t = 0;
+            }
+        } else {
+            f32 numer = c+e-b-d;
+            f32 denom = a-2*b+c;
+            s = CLAMP(numer/denom, 0, 1);
+            t = 1 - s;
+        }
+    }
+
+    out[0] = v1[0] + s * edge0[0] + t * edge1[0];
+    out[1] = v1[1] + s * edge0[1] + t * edge1[1];
+    out[2] = v1[2] + s * edge0[2] + t * edge1[2];
+}
 
 /**************************************************
  *                      WALLS                     *
@@ -33,6 +116,9 @@ static s32 find_wall_collisions_from_list(struct SurfaceNode *surfaceNode,
     register f32 y1, y2, y3;
     s32 numCols = 0;
 
+    Vec3f cPos = { 0 };
+    Vec3f cNorm = { 0 };
+
     // Max collision radius = 200
     if (radius > 200.0f) {
         radius = 200.0f;
@@ -48,66 +134,106 @@ static s32 find_wall_collisions_from_list(struct SurfaceNode *surfaceNode,
             continue;
         }
 
-        offset = surf->normal.x * x + surf->normal.y * y + surf->normal.z * z + surf->originOffset;
+        if (gServerSettings.fixCollisionBugs) {
+            // Check AABB to exclude walls before doing expensive triangle check
+            f32 minX = MIN(MIN(surf->vertex1[0], surf->vertex2[0]), surf->vertex3[0]) - radius;
+            f32 minZ = MIN(MIN(surf->vertex1[2], surf->vertex2[2]), surf->vertex3[2]) - radius;
+            f32 maxX = MAX(MAX(surf->vertex1[0], surf->vertex2[0]), surf->vertex3[0]) + radius;
+            f32 maxZ = MAX(MAX(surf->vertex1[2], surf->vertex2[2]), surf->vertex3[2]) + radius;
+            if (x < minX || x > maxX) { continue; }
+            if (z < minZ || z > maxZ) { continue; }
 
-        if (offset < -radius || offset > radius) {
-            continue;
-        }
-
-        px = x;
-        pz = z;
-
-        //! (Quantum Tunneling) Due to issues with the vertices walls choose and
-        //  the fact they are floating point, certain floating point positions
-        //  along the seam of two walls may collide with neither wall or both walls.
-        if (surf->flags & SURFACE_FLAG_X_PROJECTION) {
-            w1 = -surf->vertex1[2];            w2 = -surf->vertex2[2];            w3 = -surf->vertex3[2];
-            y1 = surf->vertex1[1];            y2 = surf->vertex2[1];            y3 = surf->vertex3[1];
-
-            if (surf->normal.x > 0.0f) {
-                if ((y1 - y) * (w2 - w1) - (w1 - -pz) * (y2 - y1) > 0.0f) {
-                    continue;
-                }
-                if ((y2 - y) * (w3 - w2) - (w2 - -pz) * (y3 - y2) > 0.0f) {
-                    continue;
-                }
-                if ((y3 - y) * (w1 - w3) - (w3 - -pz) * (y1 - y3) > 0.0f) {
-                    continue;
-                }
-            } else {
-                if ((y1 - y) * (w2 - w1) - (w1 - -pz) * (y2 - y1) < 0.0f) {
-                    continue;
-                }
-                if ((y2 - y) * (w3 - w2) - (w2 - -pz) * (y3 - y2) < 0.0f) {
-                    continue;
-                }
-                if ((y3 - y) * (w1 - w3) - (w3 - -pz) * (y1 - y3) < 0.0f) {
+            // Exclude triangles from wrong movement side
+            Vec3f norm = { surf->normal.x, surf->normal.y, surf->normal.z };
+            if (gFindWallDirectionActive) {
+                if (vec3f_dot(norm, gFindWallDirection) > 0) {
                     continue;
                 }
             }
-        } else {
-            w1 = surf->vertex1[0];            w2 = surf->vertex2[0];            w3 = surf->vertex3[0];
-            y1 = surf->vertex1[1];            y2 = surf->vertex2[1];            y3 = surf->vertex3[1];
 
-            if (surf->normal.z > 0.0f) {
-                if ((y1 - y) * (w2 - w1) - (w1 - px) * (y2 - y1) > 0.0f) {
-                    continue;
-                }
-                if ((y2 - y) * (w3 - w2) - (w2 - px) * (y3 - y2) > 0.0f) {
-                    continue;
-                }
-                if ((y3 - y) * (w1 - w3) - (w3 - px) * (y1 - y3) > 0.0f) {
-                    continue;
+            // Find closest point to triangle
+            Vec3f src = { x, y, z };
+            closest_point_to_triangle(surf, src, cPos);
+
+            // Exclude triangles where y isn't inside of it
+            if (fabs(cPos[1] - y) > 1) { continue; }
+
+            // Figure out normal
+            f32 dX = src[0] - cPos[0];
+            f32 dZ = src[2] - cPos[2];
+            f32 dist = sqrtf(dX * dX + dZ * dZ);
+            if (dist > radius) { continue; }
+
+            cNorm[0] = dX / dist;
+            cNorm[1] = 0;
+            cNorm[2] = dZ / dist;
+
+            // Exclude triangles that are colliding from the wrong side
+            if (!gFindWallDirectionActive && vec3f_dot(norm, cNorm) < 0) { continue; }
+
+        } else {
+
+            offset = surf->normal.x * x + surf->normal.y * y + surf->normal.z * z + surf->originOffset;
+
+            if (offset < -radius || offset > radius) {
+                continue;
+            }
+
+            px = x;
+            pz = z;
+
+            //! (Quantum Tunneling) Due to issues with the vertices walls choose and
+            //  the fact they are floating point, certain floating point positions
+            //  along the seam of two walls may collide with neither wall or both walls.
+            if (surf->flags & SURFACE_FLAG_X_PROJECTION) {
+                w1 = -surf->vertex1[2]; w2 = -surf->vertex2[2]; w3 = -surf->vertex3[2];
+                y1 = surf->vertex1[1];  y2 = surf->vertex2[1];  y3 = surf->vertex3[1];
+
+                if (surf->normal.x > 0.0f) {
+                    if ((y1 - y) * (w2 - w1) - (w1 - -pz) * (y2 - y1) > 0.0f) {
+                        continue;
+                    }
+                    if ((y2 - y) * (w3 - w2) - (w2 - -pz) * (y3 - y2) > 0.0f) {
+                        continue;
+                    }
+                    if ((y3 - y) * (w1 - w3) - (w3 - -pz) * (y1 - y3) > 0.0f) {
+                        continue;
+                    }
+                } else {
+                    if ((y1 - y) * (w2 - w1) - (w1 - -pz) * (y2 - y1) < 0.0f) {
+                        continue;
+                    }
+                    if ((y2 - y) * (w3 - w2) - (w2 - -pz) * (y3 - y2) < 0.0f) {
+                        continue;
+                    }
+                    if ((y3 - y) * (w1 - w3) - (w3 - -pz) * (y1 - y3) < 0.0f) {
+                        continue;
+                    }
                 }
             } else {
-                if ((y1 - y) * (w2 - w1) - (w1 - px) * (y2 - y1) < 0.0f) {
-                    continue;
-                }
-                if ((y2 - y) * (w3 - w2) - (w2 - px) * (y3 - y2) < 0.0f) {
-                    continue;
-                }
-                if ((y3 - y) * (w1 - w3) - (w3 - px) * (y1 - y3) < 0.0f) {
-                    continue;
+                w1 = surf->vertex1[0]; w2 = surf->vertex2[0]; w3 = surf->vertex3[0];
+                y1 = surf->vertex1[1]; y2 = surf->vertex2[1]; y3 = surf->vertex3[1];
+
+                if (surf->normal.z > 0.0f) {
+                    if ((y1 - y) * (w2 - w1) - (w1 - px) * (y2 - y1) > 0.0f) {
+                        continue;
+                    }
+                    if ((y2 - y) * (w3 - w2) - (w2 - px) * (y3 - y2) > 0.0f) {
+                        continue;
+                    }
+                    if ((y3 - y) * (w1 - w3) - (w3 - px) * (y1 - y3) > 0.0f) {
+                        continue;
+                    }
+                } else {
+                    if ((y1 - y) * (w2 - w1) - (w1 - px) * (y2 - y1) < 0.0f) {
+                        continue;
+                    }
+                    if ((y2 - y) * (w3 - w2) - (w2 - px) * (y3 - y2) < 0.0f) {
+                        continue;
+                    }
+                    if ((y3 - y) * (w1 - w3) - (w3 - px) * (y1 - y3) < 0.0f) {
+                        continue;
+                    }
                 }
             }
         }
@@ -147,11 +273,17 @@ static s32 find_wall_collisions_from_list(struct SurfaceNode *surfaceNode,
         //! (Wall Overlaps) Because this doesn't update the x and z local variables,
         //  multiple walls can push mario more than is required.
         //  <Fixed when gServerSettings.fixCollisionBugs != 0>
-        data->x += surf->normal.x * (radius - offset);
-        data->z += surf->normal.z * (radius - offset);
         if (gServerSettings.fixCollisionBugs) {
+            data->x = cPos[0] + cNorm[0] * radius;
+            data->z = cPos[2] + cNorm[2] * radius;
             x = data->x;
             z = data->z;
+            data->normalAddition[0] += cNorm[0];
+            data->normalAddition[2] += cNorm[2];
+            data->normalCount++;
+        } else {
+            data->x += surf->normal.x * (radius - offset);
+            data->z += surf->normal.z * (radius - offset);
         }
 
         //! (Unreferenced Walls) Since this only returns the first four walls,
diff --git a/src/engine/surface_collision.h b/src/engine/surface_collision.h
index e16becc3d..67f5c6f23 100644
--- a/src/engine/surface_collision.h
+++ b/src/engine/surface_collision.h
@@ -23,6 +23,8 @@ struct WallCollisionData
     /*0x14*/ s16 unused;
     /*0x16*/ s16 numWalls;
     /*0x18*/ struct Surface *walls[4];
+    /*????*/ Vec3f normalAddition;
+    /*????*/ u8 normalCount;
 };
 
 struct FloorGeometry
@@ -34,6 +36,9 @@ struct FloorGeometry
     f32 originOffset;
 };
 
+extern Vec3f gFindWallDirection;
+extern u8 gFindWallDirectionActive;
+
 s32 f32_find_wall_collision(f32 *xPtr, f32 *yPtr, f32 *zPtr, f32 offsetY, f32 radius);
 s32 find_wall_collisions(struct WallCollisionData *colData);
 f32 find_ceil(f32 posX, f32 posY, f32 posZ, struct Surface **pceil);
diff --git a/src/game/debug.c b/src/game/debug.c
index 528a5e30a..c94cd4724 100644
--- a/src/game/debug.c
+++ b/src/game/debug.c
@@ -4,6 +4,7 @@
 #include "debug.h"
 #include "engine/behavior_script.h"
 #include "engine/surface_collision.h"
+#include "game/level_update.h"
 #include "game_init.h"
 #include "main.h"
 #include "object_constants.h"
@@ -568,3 +569,15 @@ void debug_enemy_unknown(s16 *enemyArr) {
     enemyArr[6] = gDebugInfo[DEBUG_PAGE_ENEMYINFO][3];
     enemyArr[7] = gDebugInfo[DEBUG_PAGE_ENEMYINFO][4];
 }
+
+void debug_position(f32 x, f32 y, f32 z, bool red) {
+    struct Object* player = gMarioStates[0].marioObj;
+    if (player == NULL) { return; }
+    struct Object* obj = spawn_object(player, red ? MODEL_RED_COIN : MODEL_YELLOW_COIN, bhvSparkle);
+    if (obj != NULL) {
+        obj_scale(obj, 0.25f);
+        obj->oPosX = x;
+        obj->oPosY = y;
+        obj->oPosZ = z;
+    }
+}
\ No newline at end of file
diff --git a/src/game/debug.h b/src/game/debug.h
index 7e89296c7..b819d2fa2 100644
--- a/src/game/debug.h
+++ b/src/game/debug.h
@@ -25,4 +25,6 @@ void try_print_debug_mario_object_info(void);
 void try_do_mario_debug_object_spawn(void);
 void try_print_debug_mario_level_info(void);
 
+void debug_position(f32 x, f32 y, f32 z, bool red);
+
 #endif // DEBUG_H
diff --git a/src/game/mario.c b/src/game/mario.c
index 8a5a89a9b..d76492fb6 100644
--- a/src/game/mario.c
+++ b/src/game/mario.c
@@ -623,9 +623,9 @@ f32 vec3f_find_ceil(Vec3f pos, f32 height, struct Surface **ceil) {
 // Prevent exposed ceilings
 f32 vec3f_mario_ceil(Vec3f pos, f32 height, struct Surface **ceil) {
     if (gServerSettings.fixCollisionBugs) {
-        height = MAX(height, pos[1]);
+        height = MAX(height + 80.0f, pos[1] - 2);
     }
-    return vec3f_find_ceil(pos, height, ceil);
+    return find_ceil(pos[0], height, pos[2], ceil);
 }
 
 /**
@@ -2273,17 +2273,15 @@ void mario_update_wall(struct MarioState* m, struct WallCollisionData* wcd) {
         ? wcd->walls[wcd->numWalls - 1]
         : NULL;
 
-    vec3f_set(m->wallNormal, 0, 0, 0);
-    for (u8 i = 0; i < wcd->numWalls; i++) {
-        if (!gServerSettings.fixCollisionBugs) {
-            i = (wcd->numWalls - 1);
-        }
-        struct Surface* wall = wcd->walls[i];
-        Vec3f normal = { wall->normal.x, wall->normal.y, wall->normal.z };
-        vec3f_add(m->wallNormal, normal);
-    }
-
-    if (m->wall) {
-        vec3f_normalize(m->wallNormal);
+    if (gServerSettings.fixCollisionBugs && wcd->normalCount > 0) {
+        vec3f_set(m->wallNormal,
+                  wcd->normalAddition[0] / wcd->normalCount,
+                  wcd->normalAddition[1] / wcd->normalCount,
+                  wcd->normalAddition[2] / wcd->normalCount);
+    } else if (m->wall) {
+        vec3f_set(m->wallNormal,
+                  m->wall->normal.x,
+                  m->wall->normal.y,
+                  m->wall->normal.z);
     }
 }
diff --git a/src/game/mario_step.c b/src/game/mario_step.c
index d45e410e2..f28a8cac6 100644
--- a/src/game/mario_step.c
+++ b/src/game/mario_step.c
@@ -334,11 +334,24 @@ s32 perform_ground_step(struct MarioState *m) {
     smlua_call_event_hooks_mario_param(HOOK_BEFORE_PHYS_STEP, m);
 
     for (i = 0; i < 4; i++) {
-        intendedPos[0] = m->pos[0] + m->floor->normal.y * (m->vel[0] / 4.0f);
-        intendedPos[2] = m->pos[2] + m->floor->normal.y * (m->vel[2] / 4.0f);
-        intendedPos[1] = m->pos[1];
+        Vec3f step = {
+            m->floor->normal.y * (m->vel[0] / 4.0f),
+            0,
+            m->floor->normal.y * (m->vel[2] / 4.0f),
+        };
 
+        intendedPos[0] = m->pos[0] + step[0];
+        intendedPos[1] = m->pos[1];
+        intendedPos[2] = m->pos[2] + step[2];
+
+        vec3f_normalize(step);
+
+        vec3f_copy(gFindWallDirection, step);
+
+        gFindWallDirectionActive = true;
         stepResult = perform_ground_quarter_step(m, intendedPos);
+        gFindWallDirectionActive = false;
+
         if (stepResult == GROUND_STEP_LEFT_GROUND || stepResult == GROUND_STEP_HIT_WALL_STOP_QSTEPS) {
             break;
         }
@@ -665,11 +678,22 @@ s32 perform_air_step(struct MarioState *m, u32 stepArg) {
     m->wall = NULL;
 
     for (i = 0; i < 4; i++) {
-        intendedPos[0] = m->pos[0] + m->vel[0] / 4.0f;
-        intendedPos[1] = m->pos[1] + m->vel[1] / 4.0f;
-        intendedPos[2] = m->pos[2] + m->vel[2] / 4.0f;
+        Vec3f step = {
+            m->vel[0] / 4.0f,
+            m->vel[1] / 4.0f,
+            m->vel[2] / 4.0f,
+        };
 
+        intendedPos[0] = m->pos[0] + step[0];
+        intendedPos[1] = m->pos[1] + step[1];
+        intendedPos[2] = m->pos[2] + step[2];
+
+        vec3f_normalize(step);
+        vec3f_copy(gFindWallDirection, step);
+
+        gFindWallDirectionActive = true;
         quarterStepResult = perform_air_quarter_step(m, intendedPos, stepArg);
+        gFindWallDirectionActive = false;
 
         //! On one qf, hit OOB/ceil/wall to store the 2 return value, and continue
         // getting 0s until your last qf. Graze a wall on your last qf, and it will
diff --git a/src/game/obj_behaviors.c b/src/game/obj_behaviors.c
index 84f569b3b..c1da125d6 100644
--- a/src/game/obj_behaviors.c
+++ b/src/game/obj_behaviors.c
@@ -170,9 +170,15 @@ s8 obj_find_wall(f32 objNewX, f32 objY, f32 objNewZ, f32 objVelX, f32 objVelZ) {
         o->oPosY = hitbox.y;
         o->oPosZ = hitbox.z;
 
-        wall_nX = hitbox.walls[0]->normal.x;
-        wall_nY = hitbox.walls[0]->normal.y;
-        wall_nZ = hitbox.walls[0]->normal.z;
+        if (gServerSettings.fixCollisionBugs && hitbox.normalCount > 0) {
+            wall_nX = hitbox.normalAddition[0] / hitbox.normalCount;
+            wall_nY = hitbox.normalAddition[1] / hitbox.normalCount;
+            wall_nZ = hitbox.normalAddition[2] / hitbox.normalCount;
+        } else {
+            wall_nX = hitbox.walls[0]->normal.x;
+            wall_nY = hitbox.walls[0]->normal.y;
+            wall_nZ = hitbox.walls[0]->normal.z;
+        }
 
         objVelXCopy = objVelX;
         objVelZCopy = objVelZ;