local VERSION = 6
local muffy = {}
local directionTable = {}
local facingDirection = -1

directionTable[0] = "east"
directionTable[1] = "north"
directionTable[2] = "west"
directionTable[3] = "south"

function muffy.getVersion()
    return VERSION
end

function muffy.getFacing()
    if facingDirection >= 0 then
        return directionTable[facingDirection]
    else
        return nil
    end
end

function muffy.turnRight()
    facingDirection = facingDirection - 1
    if facingDirection < 0 then
        facingDirection = 3
    end
    print("facingDirection = "..facingDirection)
    turtle.turnRight()
end

function muffy.turnLeft()
    facingDirection = (facingDirection + 1) % 4
    turtle.turnLeft()
end

function muffy.calibrate()

    -- First get our location
    local x, y, z = gps.locate(5)
    if not x then
        return false
    end

    -- Now figure out which way we're
    --    facing
    turtle.forward()
    local x1, y1, z1 = gps.locate(5)

    -- See which variable changed
    if x1 < x then
        print("X decreased: ("..x..")->("..x1..")")
        facingDirection = 2
    elseif x1 > x then
        print("X increased: ("..x..")->("..x1..")")
        facingDirection = 0
    elseif z1 < z then
        print("Z decreased: ("..z..")->("..z1..")")
        facingDirection = 1
    elseif z1 > z then
        print("Z increased: ("..z..")->("..z1..")")
        facingDirection = 3
    else
        print("WTF DID YOU DO")
        return false
    end
    print("calibrate = "..facingDirection)
    turtle.back()
    return true
end

local function moveXAxis(dX, avoid)
    -- Move some direction along the X axis
    if dX > 0 then
        while facingDirection ~= 0 do
            muffy.turnRight()
        end
        for i=dX, 1, -1 do
            if not turtle.forward() then
                avoid()
            end
            print("dX = "..i)
        end
    elseif dX < 0 then
        while facingDirection ~= 2 do
            muffy.turnRight()
        end
        for i=dX, -1, 1 do
            if not turtle.forward() then
                avoid()
            end
            print("dX = "..i)
        end
    end
end

local function moveZAxis(dZ, avoid)
    if dZ > 0 then
        while facingDirection ~= 3 do
            muffy.turnRight()
        end
        for i=dZ, 1, -1 do
            if not turtle.forward() then
                avoid()
            end
            print("dZ = "..i)
        end
    elseif dZ < 0 then
        while facingDirection ~= 1 do
            muffy.turnRight()
        end
        for i=dZ, -1, 1 do
            if not turtle.forward() then
                avoid()
            end
            print("dZ = "..i)
        end
    end
end

local function go(dX, dY, dZ, avoid)

    -- Always move Y first
    if dY > 0 then
        for i=dY, 1, -1 do
            if not turtle.up() then
                avoid()
            end
            print("dY = "..i)
        end
    elseif dY < 0 then
        for i=dY, -1, 1 do
            if not turtle.down() then
                avoid()
            end
            print("dY = "..i)
        end
    end

    -- See if our facingDirection matches a direction we
    --    need to go
    if dX ~= 0 and (facingDirection == 0 or facingDirection == 3) then
        moveXAxis(dX, avoid)

    elseif dZ ~= 0 and (facingDirection == 1 or facingDirection == 2) then
        moveZAxis(dZ, avoid)

    else
        -- We're not facing a direction we need to go, do
        --  x first.
        moveXAxis(dX, avoid)
        moveZAxis(dZ, avoid)
    end
end


function muffy.navigate(destX, destY, destZ, avoid)

    -- Start by locating our current position
    local cX, cY, cZ = gps.locate(5)

    -- Make sure we're calibrated
    if facingDirection == -1 then
        rc = muffy.calibrate()

        -- If calibration failed, return false
        if not rc then
            print("Calibration failed!")
            return false
        end
    end

    -- Now - for the tricky part. We want to optimize
    --  our movements so that we start moving in the
    --  direction we're facing. So we need to translate
    --  the destination coordinates in (moveX, moveZ, moveY)

    while cX ~= destX or cY ~= destY or cZ ~= destZ do
        moveX = destX - cX
        moveZ = destZ - cZ
        moveY = destY - cY

        print("moveX = "..moveX)
        print("moveY = "..moveY)
        print("moveZ = "..moveZ)
        go(moveX, moveY, moveZ, avoid)
        cX, cY, cZ = gps.locate(5)
    end

    print("We got there!")
end


return muffy