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 = -20934, z = -12588},
    {x = -16832, z = -14592},
    {x = -10776, z = -13304}
}

local currentAction = "stop"
local threshold = 2  -- 角度阈值，小于这个停止转向
local arrivalDistance = 5 -- 到达目标判定距离

-- 左右转函数
local function turnLeft()
    if currentAction ~= "left" then
        relay.setOutput("left", true)
        relay.setOutput("right", false)
        currentAction = "left"
    end
end

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

local function stopTurn()
    if currentAction ~= "stop" then
        relay.setOutput("left", false)
        relay.setOutput("right", false)
        currentAction = "stop"
    end
end

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

    while true 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)

        -- 到达判定
        if distance <= arrivalDistance then
            stopTurn()
            print(string.format("✓ Waypoint %d reached", i))
            sleep(1)
            break
        end

        -- 目标角度（世界坐标，北为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
            stopTurn()
        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°", targetAngle))
        print(string.format("Relative angle (right+ / left-): %.1f°", relativeAngle))
        print(string.format("Distance to target: %.2f", distance))
        print("Current action: "..currentAction)

        sleep(0.5)
    end
end

print("\nAll waypoints reached!")
stopTurn()