home

--By xXxMoNkEyMaNxXx
--elsez
local type,next=type,next
local tonumber,tostring=tonumber,tostring

local concat=table.concat
local remove=table.remove

local abs=math.abs
local floor=math.floor
local rand=math.random

local tick=os.clock

math.randomseed(os.time())
math.random()

local arg={...}

if peripheral then
for _,side in next,{"left","right","front","back","bottom","top"} do
    if peripheral.getType(side)=="modem" then
        rednet.open(side)
    end
end
end

local nans={tostring(1/0),tostring(-1/0),tostring(-(0/0)),tostring(0/0)}
local function isnan(x)
    local s=tostring(x)
    for i,v in next,nans do
        if s==v then
            return i
        end
    end
end

local function nonenan(t)
    for i=1,#t do
        if isnan(t[i]) then
            return false,i
        end
    end
    return true
end

local function GPS(e0,e1,e2,e3,e4)
    local x0,y0,z0,t0=e0[1],e0[2],e0[3],e0[4]
    local x1,y1,z1,t1=e1[1],e1[2],e1[3],e1[4]
    local x2,y2,z2,t2=e2[1],e2[2],e2[3],e2[4]
    local x3,y3,z3,t3=e3[1],e3[2],e3[3],e3[4]
    local x4,y4,z4,t4=e4[1],e4[2],e4[3],e4[4]

    --I left brackets in some because it's easier to see the grouping
    local s1=x1-x0
    local s3=y1-y0
    local s7=z1-z0
    local s10=t1-t0
    local s2=(x2-x0)/s1
    local s5=(x3-x0)/s1
    local s9=(x4-x0)/s1
    local s6=(y2-y0)-s2*s3
    local s13=(z2-z0)-s2*s7
    local s16=(t2-t0)-s2*s10
    local s11=((y3-y0)-s5*s3)/s6
    local s17=((z3-z0)-s5*s7)-s11*s13
    local s21=((t3-t0)-s5*s10)-s11*s16
    local s15=((y4-y0)-s9*s3)/s6
    local s20=(((z4-z0)-s9*s7)-s15*s13)/s17
    local s4=x0^2+y0^2+z0^2-t0^2
    local s12=((x1^2+y1^2+z1^2-t1^2)-s4)/2
    local s14=((x2^2+y2^2+z2^2-t2^2)-s4)/2-s2*s12
    local s19=((x3^2+y3^2+z3^2-t3^2)-s4)/2-s5*s12-s11*s14

    --Calculators weren't good enough for this!  I solved it by hand ;D
    local t=-(((x4^2+y4^2+z4^2-t4^2)-s4)/2-s9*s12-s15*s14-s20*s19)/((((t4-t0)-s9*s10)-s15*s16)-s20*s21)
    local z=(s21*t+s19)/s17
    local y=(s16*t-s13*z+s14)/s6
    local x=(s10*t-s3*y-s7*z+s12)/s1
    return {floor(x+0.5),floor(y+0.5),floor(z+0.5),t}
end

local rec=rednet.receive
local broadcast=rednet.broadcast
local unserialize=textutils.unserialize
local function receive(timeout)
    local id,msg,d=rec(timeout)
    local ret=msg and unserialize(msg) or {}
    ret.senderId=id
    ret.message=msg
    ret.distance=d
    return ret
end

local function locate(timeout,period)
    timeout=timeout or 5
    period=period or 1
    local t0=tick()
    broadcast("PING")
    local p={}
    local sat={}
    repeat
        local data=receive(period)
        if #data==4 then
            local e={}
            for i=1,4 do
                local v=data[i]
                if type(v)=="number" then
                    e[i]=v
                else
                    break
                end
            end
            if #e==4 then
                sat[#sat+1]=e
            end
        elseif #data==3 and type(data.distance)=="number" then
            local e={}
            for i=1,3 do
                local v=data[i]
                if type(v)=="number" then
                    e[i]=v
                else
                    break
                end
            end
            if #e==3 then
                e[4]=-data.distance
                sat[#sat+1]=e
            end
        else
            print'Not enough hosts, needs more.'
            broadcast'PING'
        end
        if #sat>=5 then
            local chosen={}
            for i=1,5 do
                chosen[i]=remove(sat,rand(#sat))
            end
            p=GPS(unpack(chosen))
            local n=#sat
            for i=1,5 do
                sat[n+i]=chosen[i]
            end
        end
    until #p==4 and nonenan(p) and abs(p[4])<0.001 or tick()-t0>timeout
    return p
end

--[[ Working test!
local t=math.random()
local p0,p1,p2,p3,p4={math.random(),math.random(),math.random()},{math.random(),math.random(),math.random()},{math.random(),math.random(),math.random()},{math.random(),math.random(),math.random()},{math.random(),math.random(),math.random()}
local p={math.random(),math.random(),math.random(),t}
print(unpack(GPS({p0[1],p0[2],p0[3],t-math.sqrt((p[1]-p0[1])^2+(p[2]-p0[2])^2+(p[3]-p0[3])^2)},{p1[1],p1[2],p1[3],t-math.sqrt((p[1]-p1[1])^2+(p[2]-p1[2])^2+(p[3]-p1[3])^2)},{p2[1],p2[2],p2[3],t-math.sqrt((p[1]-p2[1])^2+(p[2]-p2[2])^2+(p[3]-p2[3])^2)},{p3[1],p3[2],p3[3],t-math.sqrt((p[1]-p3[1])^2+(p[2]-p3[2])^2+(p[3]-p3[3])^2)},{p4[1],p4[2],p4[3],t-math.sqrt((p[1]-p4[1])^2+(p[2]-p4[2])^2+(p[3]-p4[3])^2)})))
print(unpack(p))
--]]

local dirs={
    North=1,
    East=0,
    South=3,
    West=2,
    N=1,
    E=0,
    S=3,
    W=2,
    north=1,
    east=0,
    south=3,
    west=2,
    n=1,
    e=0,
    s=3,
    w=2,
    ["0"]=0,
    ["1"]=1,
    ["2"]=2,
    ["3"]=3,
    ["4"]=0,
}

local function setHome(v)
    local f=io.open("home.csv","w")
    f:write(concat(v,","))
    f:close()
end

local function getHome()
    local f=io.open("home.csv","r")
    if f then
        local v={}
        for a in f:read'*a':gmatch'[^%s,]+' do
            v[#v+1]=tonumber(a)
        end
        return v
    else
        error'Error: No home set.'
    end
end

if arg[1]=="set" then
    if #arg==5 then
        local x,y,z,f=unpack(arg,2,5)
        if tonumber(x) and tonumber(y) and tonumber(z) and dirs[f] then
            setHome{x,y,z,dirs[f]}
        else
            error'Invalid coordinates'
        end
    elseif #arg==2 and type(arg[2])=="string" and arg[2]~="here" then
        local x,y,z,f=arg[2]:match'(.+),(.+),(.+),(.+)'
        if tonumber(x) and tonumber(y) and tonumber(z) and dirs[f] then
            setHome{x,y,z,dirs[f]}
        end
    else
        local p=locate()
        print(concat(p,", "))
        setHome(p)
    end
else
    local p=locate()
    print(concat(p,", "))
    local h=getHome()
    print(concat(h,", "))
end