term.clear()
term.setCursorPos(1,1)
local args = table.pack(...)

local mods = {}
local ms = peripheral.getMethods("back")
for i = 1, #ms do
    mods[ms[i]] = true
end
assert(mods.getMetaOwner, "Entity Sensor & Introspection Module Required")
assert(mods.launch, "Kinetic Augment Required.")
assert(mods.getBlockMeta, "Automatic mode unavalible.")

local meta, mode, launch, yaw
local power = 0.5
local x, z = 0, 0

local laneData

do
    local file = http.get("https://p.sc3.io/api/v1/pastes/Q2EcezEjth/raw")
    laneData = textutils.unserializeJSON(file.readAll())
    file.close()

    for i = 1, #laneData.lanes-1 do
        local lanea = laneData.lanes[i]
        if not lanea.isects then
            lanea.isects = {}
        end
        for j = i+1, #laneData.lanes do
            local laneb = laneData.lanes[j]
            if not laneb.isects then
                laneb.isects = {}
            end
            if lanea.axis ~= laneb.axis and (lanea.pos >= laneb.start and lanea.pos <= laneb["end"]) then
                lanea.isects[#lanea.isects+1] = { on = laneb, pos = lanea.pos }
                laneb.isects[#laneb.isects+1] = { on = lanea, pos = laneb.pos }
            end
        end
    end
    for _, stop in ipairs(laneData.stops) do
        local found = false
        for _, lane in ipairs(laneData.lanes) do
            if lane.name == stop.on then
                -- if not lane.stops then
                --     lane.stops = {}
                -- end
                -- lane.stops[#lane.stops+1] = stop
                stop.on = lane
                found = true
                break
            end
        end
        assert(found, stop.name..", '"..tostring(stop.on).."' is not a valid lane")
    end
end

local function getxz(node)
    if node.on.axis == "z" then
        return node.on.pos, node.pos
    else
        return node.pos, node.on.pos
    end
end

local function squaredDistance(x1, z1, x2, z2)
    return math.pow(x2-x1, 2) + math.pow(z2-z1, 2)
end

local function aStar(start, goal, h)
    local openSet, cameFrom = {start}, {}
    local gScore = {
        [start] = 0
    }
    local fScore = {
        [start] = h(start)
    }

    while #openSet > 0 do
        local current, score, r
        for i, node in ipairs(openSet) do
            if not current or score > fScore[node] then
                current, score, r = node, fScore[node], i
            end
        end
        if current.on == goal.on then
            local total_path = {current, goal}
            while cameFrom[current] do
                current = cameFrom[current]
                table.insert(total_path, 1, current)
            end
            return total_path
        end
        table.remove(openSet, r)
        local x1, z1 = getxz(current)
        for i, neighbor in ipairs(current.on.isects) do
            local x2, z2 = getxz(neighbor)
            local tent_gScore = gScore[current] + squaredDistance(x1, z1, x2, z2)
            if not gScore[neighbor] or tent_gScore < gScore[neighbor] then
                cameFrom[neighbor] = current
                gScore[neighbor] = tent_gScore
                fScore[neighbor] = tent_gScore + h(neighbor)
                local add = true
                for _, node in ipairs(openSet) do
                    if node == neighbor then
                        add = false
                        break
                    end
                end
                if add then
                    openSet[#openSet+1] = neighbor
                end
            end
        end
    end
    error("Could not find path to "..goal.name)
end

local function route(to_stop, tx, tz)
    local start = {
        name = "Start"
    }
    for _, lane in ipairs(laneData.lanes) do
        if lane.pos == tz and lane.axis == "x" then
            start.on = lane
            start.pos = tx
        elseif lane.pos == tx and lane.axis == "z" then
            start.on = lane
            start.pos = tz
        end
        if start.on then
            break
        end
    end
    assert(start.on, "Not on lane? Something broke somewhere.")
    local sx, sz = getxz(to_stop)
    local route = aStar(start, to_stop, function(node)
        local nx, nz = getxz(node)
        return squaredDistance(nx, nz, sx, sz)
    end)
    return route
end

local onIce, routes = false

local function handleMetaOwner()
    while true do
        meta = peripheral.call("back", "getMetaOwner")
        x, z = x + meta.motionX, z + meta.motionZ
    end
end

local function handleRouting()
    local dest = table.concat(args):lower()
    for _, stop in ipairs(laneData.stops) do
        if dest == stop.name:lower() then
            dest = stop
            break
        end
    end
    assert(type(dest) ~= "string", "Could not find stop named"..table.concat(args))
    while true do
        if peripheral.call("back", "getBlockMeta", 0, -2, 0).name:match("slab") and peripheral.call("back", "getBlockMeta", 0, -3, 0).name == "minecraft:packed_ice" then
            if not onIce then
                routes = route(dest, math.floor(x), math.floor(z))
                -- term.setCursorPos(1,2)
                -- term.clearLine()
            end
            onIce = true
        else
            onIce = false
            -- term.setCursorPos(1,1)
            -- term.clearLine()
            -- term.setCursorPos(1,2)
            -- term.write("Not on a hyperlane. Get on lane to resume routing.")
            print("Not on a hyperlane. Get on lane to resume routing.")
        end
        sleep(0.5)
    end
end

local approach = 80 -- Start approach from 80 blocks away
local function reverse_lerp(goal, pos, vel)
    local distance = math.max(pos, goal) - math.min(pos, goal)
    if distance < approach then
        if math.abs(vel) > 0.5 then
            return 0, false
        end
        return math.max(math.pow(distance/approach, 2), distance <= 2 and 0.006 or 0.01), distance <= 0.1
    else
        return 1
    end
end

local function handleMovement()
    while true do
        if routes and onIce then
            -- term.setCursorPos(1, 1)
            -- term.write("Routing from current position")
            print("Routing from current position")
            local xAxis = table.remove(routes, 1).on.axis == 'x'
            while #routes > 0 and onIce do
                local route = table.remove(routes, 1)
                -- term.setCursorPos(1, 2)
                -- term.clearLine()
                -- term.write("Heading to "..(route.name or route.on.name))
                local gx, gz = getxz(route)
                gx, gz = gx + 0.5, gz + 0.5
                local pm = 1
                local done, get
                if xAxis then -- inverse of route point axis
                    get = function()
                        local y = 90
                        if gx-x > 0 then
                            y = y - 180
                        end
                        return gx, x, meta.motionX, y
                    end
                else
                    get = function()
                        local y = 180
                        if gz-z > 0 then
                            y = y - 180
                        end
                        return gz, z, meta.motionZ , y
                    end
                end
                print("Heading to "..(route.name or route.on.name))
                parallel.waitForAny(function() 
                        while true do
                            local g, p, v, y = get()
                            pm, done = reverse_lerp(g, p, v)
                            peripheral.call("back", "launch", y, 0, power*pm)
                        end
                end, function()
                    local prev = false
                    while true do
                        if prev and done then
                            return
                        end
                        prev = done
                        sleep(1)
                    end
                end)
                xAxis = not xAxis
            end
            -- term.setCursorPos(1, 1)
            -- term.clearLine()
            -- term.write("Arrived at destination")
            print("Arrived at destination")
            -- term.setCursorPos(1, 2)
            -- term.clearLine()
            -- term.setCursorPos(1, 2)
            sleep(1)
            return
        else
            sleep(1)
        end
    end
end

local function handleGPS()
    while true do
        local tx, _, tz = gps.locate()
        if tx then
            x, z = tx, tz
        end
        sleep(1)
    end
end

local ok, err = pcall(function()
    parallel.waitForAny(handleMetaOwner, handleRouting, handleMovement, handleGPS)
end)
if not ok then
    print(err)
    os.pullEvent("key")
end