getGPSCoordinates

function round(num, bounds)
    if(num > 0) then
        if(num + bounds >= math.ceil(num)) then
            return math.ceil(num)
        elseif(num - bounds <= math.floor(num)) then
            return math.floor(num)
        end
    elseif(num < 0) then
        if(num - bounds <= math.floor(num)) then
            return math.floor(num)
        elseif(num + bounds >= math.ceil(num)) then
            return math.ceil(num)
        end
    end
    return num
end

function testPointEquality(p1,p2)
    if(p1[2] == p2[2] and p1[3] == p2[3] and p1[4] == p2[4]) then
    return true
    else return false
    end
end

function getGPSCoordinates(timeOut)
    local timeOut = timeOut or 2
    local computer = require("computer")
    local event = require("event")
    local timer = computer.uptime()
    local points = {}
    while(computer.uptime() - timer < timeOut*4 and #points<4) do
        --print("Timer: "..computer.uptime() - timer)

        --[[    Data i assumed to be meaningful!
            This could be a huge problem, but will usually work if this
            port is only used for gps, a fix should be inplemented
        ]]--

        local eventType, recieverAddress, senderAddress, port, distance, x,y,z = event.pull(timeOut, "modem_message")
        if(distance ~= nil and x ~= nil and y ~= nil and z ~= nil) then
            local tmp = {distance, x,y,z}
            tmp = tonumberVector(tmp)
            local same = false
            for i=1, #points do
                if(testPointEquality(points[i], tmp)) then
                    same = true
                    --print("Same point!")
                    break
                end
            end
            if(not same) then
                if(#points<3) then
                    points[#points+1] = tmp
                    --print("Point added:"..tostring(tmp[1]))
                    --print(#points)
                else
                    local normPlane = getPlane(points[1],points[2],points[3])
                    --print(tostringVector(normPlane))
                    if(isInPlane(tmp,points[1],normPlane)) then
                        --print("Point in plane!")
                    else
                        --print("Point not in plane, adding point")
                        points[#points+1] = tmp
                        --print(#points)
                    end
                end
            end
        else
            print("Nil event type")
        end
    end
    --print(#points)
    if(#points >3) then
        return points
    else
        return nil
    end
end

function split(inputstr, sep)
    if(inputstr == nil or inputstr == "") then
                return nil
        end
        if sep == nil then
                sep = ","
        end
        local t={} ; i=1
        for str in string.gmatch(inputstr, "([^"..sep.."]+)") do
                t[i] = str
                i = i + 1
        end
        return t
end

function addVector(p1,p2)
    --p1 + p2
    return {p1[1]+p2[1], p1[2]+p2[2], p1[3]+p2[3]}
end

function subtractVector(p1,p2)
    --p1 - p2
    return {p1[1]-p2[1], p1[2]-p2[2], p1[3]-p2[3]}
end

function scaleVector(p1, scalar)
    --p1 * scalar
    return {p1[1]*scalar, p1[2]*scalar, p1[3]*scalar}
end

function dotVector(p1,p2)
    return p1[1]*p2[1] + p1[2]*p2[2] + p1[3]*p2[3]
end

function crossVector(p1,p2)
    return {p1[2]*p2[3] - p1[3]*p2[2], p1[3]*p2[1] - p1[1]*p2[3], p1[1]*p2[2] - p1[2]*p2[1]}
end

function magnitudeVector(p1)
    return math.sqrt(math.pow(p1[1], 2) + math.pow(p1[2], 2) + math.pow(p1[3], 2))
end

function normalizeVector(p1)
    return p1/magnitudeVector(p1)
end

function getPlane(p1,p2,p3)
    --find the equation of the plane
    local p12 = subtractVector(p2,p1)
    local p13 = subtractVector(p3,p1)
    local plane = crossVector(p12,p13)
    return plane
end

function isInPlane(p1,p2,p3)
    local bounds= 0.000001
    --[[
        where:
        p1 is the point in question,
        p2 is a point known to exist on plane p3,
        and p3 is the plane in question
    ]]--
    if(dotVector(subtractVector(p1,p2),p3) < bounds and dotVector(subtractVector(p1,p2),p3) > -bounds) then
        return true
    else
        return false
    end
end

function rotateVectorZAxis(p1,theta)
    return {p1[1]*math.cos(theta) - p1[2]*math.sin(theta), p1[1]*math.sin(theta) + p1[2]*math.cos(theta), p1[3]}
end

function rotateVectorXAxis(p1,theta)
    return {p1[1], p1[2]*math.cos(theta) - p1[3]*math.sin(theta), p1[2]*math.sin(theta) + p1[3]*math.cos(theta)}
end

function rotateVectorYAxis(p1,theta)
    return {p1[1]*math.cos(theta) + p1[3]*math.sin(theta), p1[2], p1[3]*math.cos(theta) - p1[1]*math.sin(theta)}
end

function tostringVector(p1)
    return tostring(p1[1]) .. "," .. tostring(p1[2]) .. "," .. tostring(p1[3])
end

function tonumberVector(p1)
    local tmp = {}
    for i=1, #p1 do
        tmp[i] = tonumber(p1[i])
    end
    return tmp
end

function roundVector(p1)
    local bounds = 0.000001
    local tmp = {}
    --[[
    for i=1, #p1 do
        if(p1[i] < bounds and p1[i] > -bounds) then
            tmp[i] = 0
        else
            tmp[i] = p1[i]
        end
    end
    ]]--
    for i=1, #p1 do
        tmp[i] = round(p1[i],bounds)
    end
    return tmp
end

function distanceBetweenPoints(p1,p2)
    return math.sqrt(math.pow(p1[1]-p2[1],2) + math.pow(p1[2]-p2[2],2) + math.pow(p1[3]-p2[3],2))
end

function findPossiblePoints2D(d1,p1,d2,p2)
    local distance = distanceBetweenPoints(p1,p2)
    local a = (math.pow(d1, 2) - math.pow(d2, 2) + math.pow(distance, 2)) / (2 * distance)
    --print(a)
    local intersect = addVector(p1,scaleVector(scaleVector(subtractVector(p2,p1), a), (1/distance)))
    --print(tostringVector(intersect))
    local h = math.sqrt(math.pow(d1, 2) - math.pow(a, 2))
    --print(h)
    local x_plus = intersect[1] + ((h*(p2[2]-p1[2]))/distance)
    local x_minus = intersect[1] - ((h*(p2[2]-p1[2]))/distance)
    local y_plus = intersect[2] + ((h*(p2[1]-p1[1]))/distance)
    local y_minus = intersect[2] - ((h*(p2[1]-p1[1]))/distance)
    return {x_plus,y_minus,0}, {x_minus,y_plus,0}
end

function findPossiblePoints3D(d1,p1,d2,p2,d3,p3)

    print("Trilateration points: "..d1..","..tostringVector(p1)..";"..d2..","..tostringVector(p2)..";"..d3..","..tostringVector(p3))

    --offset vectors such that p1 is at 0,0,0
    local offset = scaleVector(p1, -1)
    --print("Offset: "..tostringVector(offset))
    local p1p = addVector(p1, offset)
    --print("p1p: "..tostringVector(p1p))
    local p2p = addVector(p2, offset)
    --print("p2p: "..tostringVector(p2p))
    local p3p = addVector(p3, offset)
    --print("p3p: "..tostringVector(p3p))

    print("Offset vectors: "..tostringVector(p1p).."; "..tostringVector(p2p).."; "..tostringVector(p3p))


    --find the equation of the plane
    local plane = roundVector(getPlane(p1p,p2p,p3p))
    print("Plane: "..tostringVector(plane))

    -- checked up to here --

    --rotate first about the y axis such that p2 is at z = 0
    local angle1
    if(p2p[1] ~= 0)then
        angle1 = math.atan(p2p[3]/p2p[1])
    elseif(p2p[3] > 0) then
        --  90 degrees
        angle1 = 1.57079633
    elseif(p2p[3] < 0) then
        --  -90 degrees
        angle1 = -1.57079633
    end
    print("Angle1: "..angle1)
    local plane_ry = roundVector(rotateVectorYAxis(plane, angle1))
    print("plane_ry: "..tostringVector(plane_ry))
    local p1p_ry = roundVector(rotateVectorYAxis(p1p, angle1))
    print("p1p_ry: "..tostringVector(p1p_ry))
    local p2p_ry = roundVector(rotateVectorYAxis(p2p, angle1))
    print("p2p_ry: "..tostringVector(p2p_ry))
    local p3p_ry = roundVector(rotateVectorYAxis(p3p, angle1))
    print("p3p_ry: "..tostringVector(p3p_ry))

    --rotate second about the z axis such that p2 is at y = 0 and the line from p1 to p2 is at z=0 y=0, and is hence on the x axis
    local angle2
    if(p2p_ry[1] ~= 0)then
        angle2 = (-1) * math.atan(p2p_ry[2]/p2p_ry[1])
    elseif(p2p_ry[2] > 0) then
        --  -90 degrees
        angle2 = -1.57079633
    elseif(p2p_ry[2] < 0) then
        --  90 degrees
        angle2 = 1.57079633
    end
    print("Angle2: "..angle2)
    local plane_ryz = roundVector(rotateVectorZAxis(plane_ry, angle2))
    print("plane_ryz: "..tostringVector(plane_ryz))
    local p1p_ryz = roundVector(rotateVectorZAxis(p1p_ry, angle2))
    print("p1p_ryz: "..tostringVector(p1p_ryz))
    local p2p_ryz = roundVector(rotateVectorZAxis(p2p_ry, angle2))
    print("p2p_ryz: "..tostringVector(p2p_ryz))
    local p3p_ryz = roundVector(rotateVectorZAxis(p3p_ry, angle2))
    print("p3p_ryz: "..tostringVector(p3p_ryz))

    --rotate plane about the x axis such that p3 is at z = 0
    local angle3
    if(p3p_ryz[2] ~= 0)then
        angle3 = (-1) * math.atan(p3p_ryz[3]/p3p_ryz[2])
    elseif(p3p_ry[3] > 0) then
        --  -90 degrees
        angle3 = -1.57079633
    elseif(p3p_ry[3] < 0) then
        --  90 degrees
        angle3 = 1.57079633
    end
    print("Angle3: "..angle3)
    local plane_ryzx = roundVector(rotateVectorXAxis(plane_ryz, angle3))
    print("plane_ryzx: "..tostringVector(plane_ryzx))
    local p1p_ryzx = roundVector(rotateVectorXAxis(p1p_ryz, angle3))
    print("p1p_ryzx: "..tostringVector(p1p_ryzx))
    local p2p_ryzx = roundVector(rotateVectorXAxis(p2p_ryz, angle3))
    print("p2p_ryzx : "..tostringVector(p2p_ryzx ))
    local p3p_ryzx = roundVector(rotateVectorXAxis(p3p_ryz, angle3))
    print("p3p_ryzx : "..tostringVector(p3p_ryzx ))

    --at this point, p1 should be at 0,0,0; p2 x,0,0; and p3 should be at x,y,0

    --calculations for finding rotated, offset points
    local xp_ryzx = (math.pow(d1,2) - math.pow(d2,2) + math.pow(p2p_ryzx[1],2))/(2*p2p_ryzx[1])



    local yp_ryzx = ((math.pow(d1,2) - math.pow(d3,2) + math.pow(p3p_ryzx[1],2) + math.pow(p3p_ryzx[2],2))/(2*p3p_ryzx[2])) - ((p3p_ryzx[1]/p3p_ryzx[2])*xp_ryzx)


    local z1p_ryzx = math.sqrt(math.pow(d1,2)-math.pow(xp_ryzx,2)-math.pow(yp_ryzx,2))
    local z2p_ryzx = (-1) * math.sqrt(math.pow(d1,2)-math.pow(xp_ryzx,2)-math.pow(yp_ryzx,2))

    --possible rotated, offset points
    local point1p_ryzx = {xp_ryzx, yp_ryzx, z1p_ryzx}
    local point2p_ryzx = {xp_ryzx, yp_ryzx, z2p_ryzx}

    --rotate back around the x axis
    plane_ryx = rotateVectorXAxis(plane_ryzx, (-1)*angle3)
    local point1p_ryz = rotateVectorXAxis(point1p_ryzx, (-1)*angle3)
    local point2p_ryz = rotateVectorXAxis(point2p_ryzx, (-1)*angle3)

    --rotate back around the z axis
    plane_ry = rotateVectorZAxis(plane_ryz, (-1)*angle2)
    local point1p_ry = rotateVectorZAxis(point1p_ryz, (-1)*angle2)
    local point2p_ry = rotateVectorZAxis(point2p_ryz, (-1)*angle2)

    --rotate back around the y axis
    plane = rotateVectorYAxis(plane_ry, (-1)*angle1)
    local point1p = rotateVectorYAxis(point1p_ry, (-1)*angle1)
    local point2p = rotateVectorYAxis(point2p_ry, (-1)*angle1)
    --print(tostringVector(plane))

    --remove offset
    local point1 = addVector(point1p, scaleVector(offset,-1))
    local point2 = addVector(point2p, scaleVector(offset,-1))

    --print("Possible points are either: "..tostringVector(point1).." or "..tostringVector(point2))
    return roundVector(point1), roundVector(point2)

end

function narrow(p1,p2,d4,p4)
    local bounds = 0.00001
    local distance1 = distanceBetweenPoints(p1,p4)
    local error1 = distance1 - d4
    print(error1)
    if(error1 < bounds and error1 > -bounds) then
        return p1
    end
    local distance2 = distanceBetweenPoints(p2,p4)
    local error2 = distance2 - d4
    print(error2)
    if(error2 < bounds and error2 > -bounds) then
        return p2
    end
    print("Narrowing function could not obtain a meaningful answer")
    return nil
end

function getLocation(timeout)
    local timeout = timeout or 2
    local coords = getGPSCoordinates(timeout)
    if(coords == nil) then
        print("Coordinate table could not be obtained")
        return nil
    end
    if(#coords < 4) then
        print("Insufficient points")
        return nil
    end
    local d1 = coords[1][1]
    local p1 = {coords[1][2],coords[1][3],coords[1][4]}
    local d2 = coords[2][1]
    local p2 = {coords[2][2],coords[2][3],coords[2][4]}
    local d3 = coords[3][1]
    local p3 = {coords[3][2],coords[3][3],coords[3][4]}
    local d4 = coords[4][1]
    local p4 = {coords[4][2],coords[4][3],coords[4][4]}
    local point1, point2 = findPossiblePoints3D(d1,p1,d2,p2,d3,p3)
    print("Possible point 1: "..tostringVector(point1))
    print("Possible point 2: "..tostringVector(point2))
    local location = narrow(point1,point2,d4,p4)
    if(location ~= nil) then
        print(tostringVector(location))
        return location
    else
        print("Location could not be narrowed")
        return nil
    end
end

local term = require("term")
local component = require("component")
local event = require("event")

component.modem.open(3665)

--getGPSCoordinates(.5)
getLocation()