TrilateratorCtrl

shell.run "rm t"
shell.run "pastebin get TPT3gpNF t"
os.loadAPI "t"

local function trilaterate( A, B, C )
    local a2b = B.position - A.position
    local a2c = C.position - A.position

    if math.abs( a2b:normalize():dot( a2c:normalize() ) ) > 0.999 then
        return nil
    end

    local d = a2b:length()
    local ex = a2b:normalize( )
    local i = ex:dot( a2c )
    local ey = (a2c - (ex * i)):normalize()
    local j = ey:dot( a2c )
    local ez = ex:cross( ey )

    local r1 = A.distance
    local r2 = B.distance
    local r3 = C.distance

    local x = (r1*r1 - r2*r2 + d*d) / (2*d)
    local y = (r1*r1 - r3*r3 - x*x + (x-i)*(x-i) + j*j) / (2*j)

    local result = A.position + (ex * x) + (ey * y)

    local zSquared = r1*r1 - x*x - y*y
    if zSquared > 0 then
        local z = math.sqrt( zSquared )
        local result1 = result + (ez * z)
        local result2 = result - (ez * z)

        local rounded1, rounded2 = result1:round( 0.01 ), result2:round( 0.01 )
        if rounded1.x ~= rounded2.x or rounded1.y ~= rounded2.y or rounded1.z ~= rounded2.z then
            return rounded1, rounded2
        else
            return rounded1
        end
    end
    return result:round( 0.01 )
end

local function narrow( p1, p2, fix )
    local dist1 = math.abs( (p1 - fix.position):length() - fix.distance )
    local dist2 = math.abs( (p2 - fix.position):length() - fix.distance )

    if math.abs(dist1 - dist2) < 0.01 then
        return p1, p2
    elseif dist1 < dist2 then
        return p1:round( 0.01 )
    else
        return p2:round( 0.01 )
    end
end

t.on_all_modems(t.wired, "open", t.message_channel)
t.on_all_modems(t.wireless, "open", 31415)

local function send_command(c)
    t.on_all_modems(t.wired, "transmit", t.command_channel, t.message_channel, c)
end

local function message_matches(last_fix, message)
    if not last_fix then return true end
    return type(last_fix.message) == type(message.message) and last_fix.channel == message.channel and last_fix.reply_channel == message.reply_channel
end

local function do_trilateration()
    while true do
        local fixes = {}

        local already_has_count = 0

        repeat
            local _, _, chan, reply_chan, message = os.pullEvent "modem_message"

            local last_fix = fixes[#fixes]

            if chan == t.message_channel and type(message) == "table" and message.position and message.ID and message.distance and message_matches(last_fix, message) then
                print("fix from", message.ID, #fixes)

                local already_has_fix = false
                for _, f in pairs(fixes) do
                    if f.ID == message.ID then already_has_fix = true already_has_count = already_has_count + 1 end
                end

                if already_has_count >= 2 then
                    fixes = {}
                    already_has_count = 0
                    print "Switching to next message."
                end

                message.position = vector.new(table.unpack(message.position))

                if not already_has_fix then table.insert(fixes, message)
                else print("Already have fix from", message.ID) end
            end
        until #fixes == 4

        local first = fixes[1]
        local p1, p2 = trilaterate(fixes[1], fixes[2], fixes[3])
        if p1 and p2 then
            local pos = narrow(p1, p2, fixes[4])
            local first_line = tostring(first.channel) .. " > " .. tostring(first.reply_channel)
            local second_line = textutils.serialise(first.message) .. " | " .. textutils.serialise(pos)
            print(first_line)
            print(second_line)
            t.on_all_modems(t.wireless, "transmit", 31415, t.command_channel, { type = "position fix", origin = "GTech Trilaterator", position = pos, channel = first.channel, replyChannel = first.reply_channel })
        end
    end
end

local blacklist = {
    [666] = true,
    [999] = true
}

local function switch_channels()
    local channels = {}
    while true do
        local _, _, chan, reply_chan, message = os.pullEvent "modem_message"
        if chan == 31415 and type(message) == "table" and message.senderChannel then
            local new_c = message.senderChannel
            local already_exists = false
            for _, c in pairs(channels) do
                if c == new_c then print("Already listening on", c) already_exists = true break end
            end
            if not already_exists and not blacklist[new_c] and not blacklist[message.replyChannel] then
                print("Listening on channel", new_c)
                table.insert(channels, new_c)
                if #channels == 127 then
                    print("Dropping channel", table.remove(channels, 1))
                end
                send_command {
                    type = "set_channels",
                    channels = channels
                }
            end
        end
    end
end

parallel.waitForAll(do_trilateration, switch_channels)