-- ==========================================
-- CONFIG
-- ==========================================
local ROUTE_NAME = "AE Express"
local API_URL = "https://map.earthcivils.top/route-map/route-api"
-- ==========================================
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

-- Waypoints (full route)
local waypoints = {
    {x = -2232, z = -12121, waitTime = 0},
    {x = -2234, z = -12453, waitTime = 0},
    {x = -2496, z = -12587, waitTime = 600},
    {x = -2948, z = -12212, waitTime = 0},
    {x = -8929, z = -11816, waitTime = 0},
    {x = -17232, z = -14640, waitTime = 0},
    {x = -19816, z = -15584, waitTime = 0},
    {x = -21360, z = -15208, waitTime = 600},
    {x = -22048, z = -14184, waitTime = 0},
    {x = -25575, z = -12897, waitTime = 0},
    {x = -25723, z = -12718, waitTime = 0},
    {x = -25812, z = -12589, waitTime = 600},
    {x = -25592, z = -12120, waitTime = 0},
    {x = -23288, z = -9576, waitTime = 0},
    {x = -24168, z = -7308, waitTime = 0},
    {x = -24028, z = -6910, waitTime = 0},
    {x = -23829, z = -6751, waitTime = 600},
    {x = -22816, z = -6680, waitTime = 0}
}

local currentAction = "forward"

-- ==========================================
-- API post function
-- ==========================================
local function pushToAPI(status)
    local pos = ship.getWorldspacePosition()
    local data = {
        routes = {{
            name = ROUTE_NAME,
            checkpoints = waypoints,
            shipPosition = { x = math.floor(pos.x), z = math.floor(pos.z) },
            status = status
        }}
    }
    local jsonData = textutils.serializeJSON(data)
    local r = http.post(API_URL, jsonData, {["Content-Type"] = "application/json"})
    if r then r.close() end
end

-- ==========================================
-- Ship control
-- ==========================================
local function moveForward()
    if currentAction ~= "forward" then
        relay.setOutput("left", false)
        relay.setOutput("back", false)
        relay.setOutput("top", false)
        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)
    currentAction = "stop"
end

-- ==========================================
-- ASCII compass
-- ==========================================
local function drawCompass(yaw, targetAngle)
    local width = 41
    local half = math.floor(width / 2)
    local relativeAngle = targetAngle - yaw
    if relativeAngle > 180 then relativeAngle = relativeAngle - 360 end
    if relativeAngle < -180 then relativeAngle = relativeAngle + 360 end
    local offset = math.floor(relativeAngle / (180 / half))
    local compass = {}

    for i = -half, half do
        if i == 0 then table.insert(compass, "^")
        elseif i == offset then table.insert(compass, "*")
        elseif i % 5 == 0 then table.insert(compass, "|")
        else table.insert(compass, "-") end
    end

    print("Compass:")
    print(table.concat(compass))
    print("[^ ship | * target]")
end

-- ==========================================
-- Find nearest start point (1000 block rule)
-- ==========================================
local function findStartingWaypoint()
    local pos = ship.getWorldspacePosition()
    local nearestIndex, nearestDist = 1, math.huge
    for i, wp in ipairs(waypoints) do
        local dx, dz = wp.x - pos.x, wp.z - pos.z
        local dist = math.sqrt(dx*dx + dz*dz)
        if dist < nearestDist then nearestDist, nearestIndex = dist, i end
    end
    print("Nearest WP: #"..nearestIndex.." dist "..nearestDist)
    if nearestDist <= 1000 then
        return math.min(nearestIndex + 1, #waypoints)
    else
        return nearestIndex
    end
end

local function reverseWaypoints()
    local i, j = 1, #waypoints
    while i < j do waypoints[i], waypoints[j] = waypoints[j], waypoints[i] i=i+1 j=j-1 end
end

-- ==========================================
-- MAIN LOOP — round trip forever
-- ==========================================
local startIndex = findStartingWaypoint()

while true do
    for i = startIndex, #waypoints do
        local wp = waypoints[i]
        local arrived = false

        while not arrived do
            local pos = ship.getWorldspacePosition()
            local yaw360
            do
                local e = ship.getEulerAnglesYXZ()
                local yaw = e.y
                yaw360 = (270 - math.deg(yaw)) % 360  -- DO NOT CHANGE
            end

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

            -- ANGLE MATH — DO NOT CHANGE
            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

            -- Dynamic threshold
            local threshold = (distance > 200) and 8 or 3
            local interval = (distance > 200) and 2 or 0.5
            local arrivalDist = ((i == 1) or (i == #waypoints)) and 16 or 50

            -- ===== ARRIVAL =====
            if distance <= arrivalDist then
                stopAll()
                print("✓ Reached WP "..i)

                -- ✅ set idle when reach ANY waypoint
                pushToAPI("idle")

                sleep(wp.waitTime or 0)
                sleep(2)
                arrived = true
            else
                -- ===== Steering (UNCHANGED) =====
                if math.abs(relativeAngle) < threshold then moveForward()
                elseif relativeAngle > 0 then turnRight()
                else turnLeft() end

                pushToAPI("active")
            end

            -- HUD
            term.clear()
            term.setCursorPos(1,1)
            print(string.format("WP %d/%d", i, #waypoints))
            print(string.format("Pos: %.1f, %.1f", pos.x, pos.z))
            print(string.format("Yaw: %.1f  Target: %.1f", yaw360, targetAngle))
            print(string.format("Rel: %.1f Dist: %.1f", relativeAngle, distance))
            print("Action:", currentAction)
            print("")
            drawCompass(yaw360, targetAngle)
            sleep(interval)
        end
    end

    -- reverse and go back
    print("Leg complete — reversing route.")
    reverseWaypoints()
    startIndex = 1
end