LM_Pathfinder

-- LatvianModder's Pathfinding code
running = true
homePos = nil
pos = nil
--[[ 0 = z-, 1 = x+, 2 = z+, 3 = x- ]]--
rotation = 0

function resetTerm()
    term.clear()
    term.setCursorPos(1, 1)
end

function init()
    resetTerm()

    rednet.open("left")

    homePos = vector.new(0, 0, 0)
    pos = vector.new(0, 0, 0)

    printNet("Turtle inited! 'help' for more info")
end

function loop()
    event, p1, p2, p3 = os.pullEvent()

    if event == "key" then
        if p1 == 41 then
            running = false
        end
    end

    if event == "rednet_message" then
        params = split(p2, " ")

        num = #params

        if num >= 2 and params[1] == "latpath" then
            --print("Received wireless command from " .. p1 .. ": " .. p2)
            if num == 2 then
                if params[2] == "reboot" then
                    printNet("Rebooting...")
                    return os.reboot()
                end

                if params[2] == "home" then
                    printNet("Going home...")
                    return moveAbs(homePos)
                end

                if params[2] == "refuel" then
                    printNet("Fuel added")
                    return turtle.refuel(1)
                end

                if params[2] == "refuel_all" then
                    printNet("All fuel added")
                    return shell.run("refuel all")
                end

                if params[2] == "getpos" then
                    return printNet("Pos: " .. pos.x .. ", " .. pos.y .. ", " .. pos.z)
                end

                if params[2] == "homedist" then
                    return printNet("Dist from home: " .. getDist(pos, homePos));
                end

                if params[2] == "ff" then
                    return t_ff()
                end

                if params[2] == "rl" then
                    return t_rotLeft()
                end

                if params[2] == "rr" then
                    return t_rotRight()
                end

                if params[2] == "bb" then
                    return t_bb()
                end

                if params[2] == "uu" then
                    return t_up()
                end

                if params[2] == "dd" then
                    return t_down()
                end

                if params[2] == "rot0" then
                    return t_rot0()
                end

                if params[2] == "getrot" then
                    return printNet("Rotation: " .. rotation)
                end

                if params[2] == "setdefrot" then
                    rotation = 0
                    return true
                end

                if params[2] == "help" then
                    printNet("reboot - Restartet")
                    printNet("refuel - Refuel 1 coal")
                    printNet("refuel_all - Refuel all")
                    printNet("getpos - Pozicija")
                    printNet("getrot - Rotacija")
                    printNet("setdefrot - Nofixet rotaciju")
                    printNet("home - Braukt uz majam")
                    printNet("homedist - Attalums lidz sakumpunktam")
                    printNet("ff, bb, rl, rr, uu, dd - Braukt, griezties")
                    printNet("ff, bb <x> - Braukt uz prieksu/atpakal x metrus")
                    printNet("goto <x> <y> <z> - Braukt uz absolutam x y z")
                    printNet("goto_rel <x> <y> <z> - Braukt uz relativam x y z")
                    return
                end

                printNet("Unknown cmd: " .. params[2])
            end

            if num == 3 then
                if params[2] == "ff" then
                    return t_ff_dist(params[3] + 0)
                end

                if params[2] == "bb" then
                    t_rot180()
                    ff = t_ff_dist(params[3] + 0)
                    t_rot180()
                    return ff
                end
            end

            if num == 5 then
                if params[2] == "goto" then
                    x = params[3] + 0
                    y = params[4] + 0
                    z = params[5] + 0
                    return moveAbs(vector.new(x, y, z))
                end

                if params[2] == "goto_rel" then
                    x = params[3] + 0
                    y = params[4] + 0
                    z = params[5] + 0
                    return moveRel(vector.new(x, y, z))
                end
            end
        end
    end
end

function printNet(txt)
    print(txt)
    rednet.broadcast("latprint " .. txt)
end

function getDist(p1, p2)
    return (p1:sub(p2)):length()
end

function t_rot0()
    if rotation == 0 then return end
    if rotation == 1 then return t_rotLeft() end
    if rotation == 3 then return t_rotRight() end
    t_rotLeft() t_rotLeft() return true
end

function moveAbs(p)
    return moveRel(p - pos)
end

function moveRel(p)
    if p == nil or ( p.x == 0 and p.y == 0 and p.z == 0 )
    then return false end

    t_rot0()

    if p.z ~= 0 then
        invXZ = false
        if p.z > 0 then
            t_rot180()
            p.z = -p.z
            --p.x = -p.x
            invXZ = true
        end

        t_ff_dist(-p.z)

        if invXZ then
            t_rot180()
        end
    end

    if p.x ~= 0 then
        if p.x > 0 then
            t_rotRight()
            t_ff_dist(p.x)
        else
            t_rotLeft()
            t_ff_dist(-p.x)
        end
    end

    t_toAbsY(p.y)

    t_rot0()
end

function refuel()
    --if turtle.refuel(0) then
        turtle.refuel(1)
    --end
end

function t_up()
    refuel()
    if turtle.up() then
        pos.y = pos.y + 1
        return true
    end

    return false
end

function t_down()
    refuel()
    if turtle.down() then
        pos.y = pos.y - 1
        return true
    end

    return false
end

function t_ff()
    refuel()
    if turtle.forward() then
        if rotation == 0 then
            pos.z = pos.z - 1
        end

        if rotation == 1 then
            pos.x = pos.x + 1
        end

        if rotation == 2 then
            pos.z = pos.z + 1
        end

        if rotation == 3 then
            pos.x = pos.x - 1
        end

        return true
    end

    return false
end

function t_rotLeft()
    if not turtle.turnLeft() then return false end
    rotation = rotation - 1
    if rotation == -1 then
    rotation = 3 end
    return true
end

function t_rotRight()
    if not turtle.turnRight() then return false end
    rotation = rotation + 1
    if rotation == 4 then
    rotation = 0 end
    return true
end

function t_rot180()
    t_rotLeft()
    t_rotLeft()
end

function t_bb()
    t_rot180()
    ff = t_ff()
    t_rot180()
    return ff
end

function t_ff_dist(dist)
    timesUp = 0
    for i = 1, dist, 1 do
        while not t_ff() do
            t_up()
            timesUp = timesUp + 1
        end
    end

    if timesUp > 0 then
        for i = 1, timesUp, 1 do
            t_down()
        end
    end
end

function t_bb_dist(dist)
    t_rot180()
    t_ff_dist(dist)
    t_rot180()
end

function t_toAbsY(y)
    return t_toRelY(y - pos.y)
end

function t_toRelY(y)
    moved = 0
    if y == 0 then return 0 end
    if y > 0 then
        for i = 1, y, 1 do
            if t_up() then
                moved = moved + 1
            end
        end
    end

    for i = 1, -y, 1 do
        if t_down() then
            moved = moved + 1
        end
    end

    return moved
end

-- String Utils

function split(s, regex)
    outResults = { }
    local theStart = 1
    local theSplitStart, theSplitEnd = string.find( s, regex, theStart )
    while theSplitStart do
        table.insert( outResults, string.sub( s, theStart, theSplitStart-1 ) )
        theStart = theSplitEnd + 1
        theSplitStart, theSplitEnd = string.find( s, regex, theStart )
    end
    table.insert( outResults, string.sub( s, theStart ) )
    return outResults
end

function stringStarts(s, st)
   return string.sub(s, 1, string.len(st)) == st
end

-- Loop

init()
while running do
    loop()
end

resetTerm()