diff --git a/turtle.lua b/turtle.lua
index f3de564..e92113f 100644
--- a/turtle.lua
+++ b/turtle.lua
@@ -203,7 +203,215 @@ end
 
 syncHomeWithServer()
 updateFuel()
-print("Ready! Turtle " .. os.getComputerID() .. " online (v5 server-driven)")
+
+-- ========== Movement Wrapping (heading + position tracking) ==========
+
+-- Heading: 0=south(+z), 1=west(-x), 2=north(-z), 3=east(+x)
+local MOVE_DELTA = {
+    [0] = { x = 0, z = 1 },    -- south
+    [1] = { x = -1, z = 0 },   -- west
+    [2] = { x = 0, z = -1 },   -- north
+    [3] = { x = 1, z = 0 },    -- east
+}
+
+local _rawForward   = turtle.forward
+local _rawBack      = turtle.back
+local _rawUp        = turtle.up
+local _rawDown      = turtle.down
+local _rawTurnLeft  = turtle.turnLeft
+local _rawTurnRight = turtle.turnRight
+
+turtle.forward = function()
+    local ok, reason = _rawForward()
+    if ok and state.position then
+        local d = MOVE_DELTA[state.facing]
+        state.position.x = state.position.x + d.x
+        state.position.z = state.position.z + d.z
+    end
+    return ok, reason
+end
+
+turtle.back = function()
+    local ok, reason = _rawBack()
+    if ok and state.position then
+        local d = MOVE_DELTA[state.facing]
+        state.position.x = state.position.x - d.x
+        state.position.z = state.position.z - d.z
+    end
+    return ok, reason
+end
+
+turtle.up = function()
+    local ok, reason = _rawUp()
+    if ok and state.position then
+        state.position.y = state.position.y + 1
+    end
+    return ok, reason
+end
+
+turtle.down = function()
+    local ok, reason = _rawDown()
+    if ok and state.position then
+        state.position.y = state.position.y - 1
+    end
+    return ok, reason
+end
+
+turtle.turnLeft = function()
+    local ok = _rawTurnLeft()
+    if ok then
+        state.facing = (state.facing + 3) % 4
+        _G._turtleFacing = state.facing
+    end
+    return ok
+end
+
+turtle.turnRight = function()
+    local ok = _rawTurnRight()
+    if ok then
+        state.facing = (state.facing + 1) % 4
+        _G._turtleFacing = state.facing
+    end
+    return ok
+end
+
+-- Detect heading via GPS triangulation
+local function detectHeading()
+    local x1, _, z1 = gps.locate(2)
+    if not x1 then return false end
+    if _rawForward() then
+        local x2, _, z2 = gps.locate(2)
+        _rawBack()
+        if x2 then
+            local dx = math.floor(x2) - math.floor(x1)
+            local dz = math.floor(z2) - math.floor(z1)
+            if dz > 0 then state.facing = 0       -- south
+            elseif dz < 0 then state.facing = 2    -- north
+            elseif dx > 0 then state.facing = 3    -- east
+            elseif dx < 0 then state.facing = 1    -- west
+            end
+            _G._turtleFacing = state.facing
+            return true
+        end
+    end
+    return false
+end
+
+if state.position then
+    if detectHeading() then
+        print("Heading: " .. ({"south","west","north","east"})[state.facing + 1])
+    else
+        print("Heading: unknown (blocked)")
+    end
+end
+
+-- ========== Pathfinding Module ==========
+
+local pathfind = {}
+
+--- Face a target heading.
+function pathfind.face(targetH)
+    targetH = targetH % 4
+    while state.facing ~= targetH do
+        local diff = (targetH - state.facing) % 4
+        if diff == 1 then
+            turtle.turnRight()
+        elseif diff == 3 then
+            turtle.turnLeft()
+        else
+            turtle.turnRight()
+            turtle.turnRight()
+        end
+    end
+end
+
+--- Navigate to target coordinates with simple obstacle avoidance.
+-- @param tx, ty, tz  target position
+-- @param options     { dig = false, maxAttempts = 256 }
+-- @return true or false, error
+function pathfind.goto(tx, ty, tz, options)
+    options = options or {}
+    local maxAttempts = options.maxAttempts or 256
+    local dig = options.dig or false
+    local attempts = 0
+
+    while attempts < maxAttempts do
+        local pos = state.position
+        if not pos then return false, "No GPS position" end
+
+        -- Arrived?
+        if pos.x == tx and pos.y == ty and pos.z == tz then
+            return true
+        end
+
+        attempts = attempts + 1
+        local moved = false
+
+        -- Priority: Y first (get to correct height), then X, then Z
+        if not moved and pos.y ~= ty then
+            if pos.y < ty then
+                moved = turtle.up()
+                if not moved and dig then turtle.digUp(); moved = turtle.up() end
+            else
+                moved = turtle.down()
+                if not moved and dig then turtle.digDown(); moved = turtle.down() end
+            end
+        end
+
+        if not moved and pos.x ~= tx then
+            pathfind.face(tx > pos.x and 3 or 1)
+            moved = turtle.forward()
+            if not moved and dig then turtle.dig(); moved = turtle.forward() end
+        end
+
+        if not moved and pos.z ~= tz then
+            pathfind.face(tz > pos.z and 0 or 2)
+            moved = turtle.forward()
+            if not moved and dig then turtle.dig(); moved = turtle.forward() end
+        end
+
+        -- Obstacle avoidance: try going around
+        if not moved then
+            if turtle.up() then
+                moved = true
+            elseif turtle.down() then
+                moved = true
+            else
+                turtle.turnRight()
+                if turtle.forward() then
+                    moved = true
+                else
+                    turtle.turnLeft()
+                    turtle.turnLeft()
+                    if turtle.forward() then
+                        moved = true
+                    else
+                        turtle.turnRight() -- restore heading
+                        return false, "Stuck at " .. pos.x .. "," .. pos.y .. "," .. pos.z
+                    end
+                end
+            end
+        end
+    end
+
+    return false, "Max attempts exceeded"
+end
+
+--- Go home (to saved home position).
+function pathfind.goHome(options)
+    if not state.homePosition then return false, "No home position set" end
+    return pathfind.goto(state.homePosition.x, state.homePosition.y, state.homePosition.z, options)
+end
+
+--- Get current heading name.
+function pathfind.headingName()
+    return ({"south","west","north","east"})[state.facing + 1]
+end
+
+-- Expose globally for eval access
+_G._pathfind = pathfind
+
+print("Ready! Turtle " .. os.getComputerID() .. " online (v5 server-driven + pathfinding)")
 broadcastStatus()
 
 -- ========== Main Loop ==========