local relay = peripheral.find("redstone_relay")

if not ship then error("Ship peripheral not found!") end
if not relay then error("Redstone relay not found!") end

-- 航点列表
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("left", false)
        relay.setOutput("back", false)
        relay.setOutput("top", false)   -- 保持 forward
        currentAction = "forward"
    end
end

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

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

local function stopAll()
    relay.setOutput("left", false)
    relay.setOutput("back", false)
    relay.setOutput("top", true)  -- 停止 forward
    currentAction = "stop"
end

-- 主循环，依次导航航点
for i, waypoint in ipairs(waypoints) do
    print(string.format("Navigating to waypoint %d: (%.2f, %.2f)", i, waypoint.x, waypoint.z))

    local arrived = false
    while not arrived do
        local pos = ship.getWorldspacePosition()
        local euler = ship.getEulerAnglesYXZ()
        local yaw = euler.y
        local yawDeg = math.deg(yaw)
        local yaw360 = (270 - yawDeg) % 360  -- 北=0，顺时针增大

        local dx = waypoint.x - pos.x
        local dz = waypoint.z - pos.z
        local distance = math.sqrt(dx*dx + dz*dz)

        -- 根据距离调整阈值和更新间隔
        local threshold, updateInterval
        if distance > 200 then
            threshold = 20
            updateInterval = 10
        else
            threshold = 10
            updateInterval = 1
        end

        -- 根据航点位置调整到达判定距离
        local arrivalDistance
        if i == 1 or i == #waypoints then
            arrivalDistance = 16
        else
            arrivalDistance = 50
        end

        -- 到达判定
        if distance <= arrivalDistance then
            stopTurn()
            arrived = true
        else
            -- 目标角度（世界坐标，北为0°）
            local targetAngle = math.deg(math.atan2(dx, -dz))
            if targetAngle < 0 then targetAngle = targetAngle + 360 end

            -- 相对于船头的偏转角（右正左负）
            local relativeAngle = targetAngle - yaw360
            if relativeAngle > 180 then relativeAngle = relativeAngle - 360 end
            if relativeAngle < -180 then relativeAngle = relativeAngle + 360 end

            -- 控制左右转
            if relativeAngle > threshold then
                turnRight()
            elseif relativeAngle < -threshold then
                turnLeft()
            else
                moveForward()
            end
        end

        -- 打印状态
        term.clear()
        term.setCursorPos(1,1)
        print(string.format("Waypoint %d/%d", i, #waypoints))
        print(string.format("Pos: (%.2f, %.2f)", pos.x, pos.z))
        print(string.format("Yaw 0~360°: %.1f°", yaw360))
        print(string.format("Target angle: %.1f°", math.deg(math.atan2(dx, -dz))))
        print(string.format("Relative angle (right+ / left-): %.1f°", relativeAngle or 0))
        print(string.format("Distance to target: %.2f", distance))
        print("Current action: "..currentAction)
        print(string.format("Threshold: %d°, Update interval: %ds, Arrival distance: %.1f", threshold, updateInterval, arrivalDistance))

        sleep(updateInterval)
    end
end

-- 到达最后一个航点后停止
stopAll()
print("\nAll waypoints reached! Ship stopped.")