local relay = peripheral.find("redstone_relay")

local waypoints = {
    {x = 364, z = 139},
    {x = -125, z = 38},
    {x = 177, z = -368}
}

local currentAction = "forward"

local function moveForward()
    if currentAction ~= "forward" then
        relay.setOutput("top", false)
        relay.setOutput("back", false)
        relay.setOutput("left", false)
        currentAction = "forward"
    end
end

local function turnLeft()
    if currentAction ~= "left" then
        relay.setOutput("top", false)
        relay.setOutput("back", false)
        relay.setOutput("left", true)
        currentAction = "left"
    end
end

local function turnRight()
    if currentAction ~= "right" then
        relay.setOutput("top", false)
        relay.setOutput("left", false)
        relay.setOutput("back", true)
        currentAction = "right"
    end
end

local function stopEngine()
    if currentAction ~= "stop" then
        relay.setOutput("top", true)
        relay.setOutput("back", false)
        relay.setOutput("left", false)
        currentAction = "stop"
    end
end

local function normalizeAngle(angle)
    while angle > math.pi do angle = angle - 2*math.pi end
    while angle < -math.pi do angle = angle + 2*math.pi end
    return angle
end

local function navigateToPoint(currentX, currentZ, currentYaw, targetX, targetZ)
    local arrivalDistance = 20
    local angleThreshold = math.rad(15)

    local dx = targetX - currentX
    local dz = targetZ - currentZ
    local distance = math.sqrt(dx*dx + dz*dz)

    if distance < arrivalDistance then
        stopEngine()
        return true
    end

    -- FIXED: Use -dx and -dz to correct coordinate system
    local targetHeading = math.atan2(-dx, -dz)

    local headingDiff = normalizeAngle(targetHeading - currentYaw)

    if math.abs(headingDiff) < angleThreshold then
        moveForward()
    elseif headingDiff > 0 then
        turnLeft()
    else
        turnRight()
    end

    return false, headingDiff, targetHeading
end

print("======================================")
print("  Valkyrien Skies Auto Navigation")
print("======================================")

if not ship then
    print("Error: Ship peripheral not found!")
    return
end

if not relay then
    print("Error: Redstone relay not found!")
    return
end

print("System initialized")
print(string.format("Total waypoints: %d", #waypoints))
print("\nPress any key to start navigation...")
os.pullEvent("key")

for i, waypoint in ipairs(waypoints) do
    print(string.format("\n========== Waypoint %d/%d ==========", i, #waypoints))
    print(string.format("Target: (%d, %d)", waypoint.x, waypoint.z))

    local arrived = false

    while not arrived do
        local pos = ship.getWorldspacePosition()
        local euler = ship.getEulerAnglesYXZ()
        local yaw = euler.y

        local arr, headingDiff, targetHeading = navigateToPoint(
            pos.x, pos.z,
            yaw,
            waypoint.x, waypoint.z
        )
        arrived = arr

        local dx = waypoint.x - pos.x
        local dz = waypoint.z - pos.z
        local distance = math.sqrt(dx*dx + dz*dz)
        print(string.format("Pos:(%.0f,%.0f) Dist:%.0f Yaw:%.1f° Target:%.1f° Dev:%.1f° [%s]",
            pos.x, pos.z, distance, math.deg(yaw), math.deg(targetHeading), math.deg(headingDiff), currentAction))

        sleep(0.5)
    end

    print(string.format("✓ Waypoint %d reached", i))

    if i < #waypoints then
        print("Waiting for 3 seconds...")
        sleep(3)
    end
end

print("\n======================================")
print("  All waypoints completed!")
print("======================================")
stopEngine()