-- Turtle Self-tracking System created by Latias1290.

local xPos, yPos, zPos = nil
local face = 1
local cal = false

Direction = {
    West = 1,
    North = 2,
    East = 3,
    South = 4
}

function setLocation() -- get gps using other computers
    -- calculate facing direction
    face = getFacing()

    -- set coords after facing has been found out
    xPos, yPos, zPos = gps.locate()

    -- set cabllibrated to true
    cal = true
end

function getFacing()
    local loc1 = vector.new(gps.locate())
    if not turtle.forward() then
        for i = 0, 9 do
            if turtle.forward() then
                break
            else
                turtle.dig()
            end
        end
    end

    local loc2 = vector.new(gps.locate())
    local heading = loc2 - loc1

    return ((heading.x + math.abs(heading.x) * 2) + (heading.z + math.abs(heading.z) * 3))
end

function manSetLocation(x, y, z) -- manually set location
    xPos = x
    yPos = y
    zPos = z
    cal = true
end

function getLocation() -- return the location
    if xPos ~= nil then
        return xPos, yPos, zPos, face
    else
        return nil
    end
end

function getDirFromMotion(motion)
    local xMot = motion[1]
    local zMot = motion[2]

    if xMot == 1 and zMot == 0 then
        return Direction.East
    elseif xMot == -1 and zMot == 0 then
        return Direction.West
    elseif xMot == 0 and zMot == 1 then
        return Direction.South
    elseif xMot == 0 and zMot == -1 then
        return Direction.North
    end
end

function getMotion(dir)
    if dir == Direction.East then
        return {1, 0}
    elseif dir == Direction.West then
        return {-1, 0}
    elseif dir == Direction.South then
        return {0, 1}
    elseif dir == Direction.North then
        return {0, -1}
    end
end

function faceDir(dir)
    if dir == 1 and face == 4 then
        turnRight()
    elseif dir == 4 and face == 1 then
        turnLeft()
    else
        local diff = dir - face
        while diff > 0 do
            turnRight()
            diff = diff - 1
        end

        while diff < 0 do
            turnLeft()
            diff = diff + 1
        end
    end

    print("After doing corrections: " .. face)
end

function turnLeft() -- turn left
    if not cal then
        print("Not Calibrated")
        return
    end

    turtle.turnLeft()

    face = face - 1
    if face <= 0 then
        face = 4
    end
end

function turnRight() -- turn right
    if not cal then
        print("Not Calibrated")
        return
    end

    turtle.turnRight()

    face = face + 1
    if face >= 5 then
        face = 0
    end
end

function forward() -- go forward
    if not cal then
        print("Not Calibrated")
        return
    end

    if turtle.forward() then
        local mot = getMotion(face)
        xPos = xPos + mot[1]
        zPos = zPos + mot[2]
    end
end

function back() -- go back
    if not cal then
        print("Not Calibrated")
        return
    end

    if turtle.back() then
        local mot = getMotion(face)
        xPos = xPos - mot[1]
        zPos = zPos - mot[2]
    end
end

function up() -- go up
    if not cal then
        print("Not Calibrated")
        return
    end

    if turtle.up() then
        yPos = yPos + 1
    end
end

function down() -- go down
    if not cal then
        print("Not Calibrated")
        return
    end

    if turtle.down() then
        yPos = yPos - 1
    end
end

function jump() -- perform a jump. useless? yup!
    up()
    down()
end

local scanner_radius = 8
local scanner_width = scanner_radius * 2 + 1

local function scanned_at(x, y, z, scanned)
    return scanned[scanner_width ^ 2 * (x + scanner_radius) + scanner_width * (y + scanner_radius) + (z + scanner_radius) + 1]
end

local function heuristic(nodeA, nodeB)
    return math.sqrt((nodeB.x - nodeA.x)^2 + (nodeB.y - nodeA.y)^2 + (nodeB.z - nodeA.z)^2)
end

local function nodeObject(vec, gScore, fScore, block)
    return {vec=vec, gScore=gScore, fScore=fScore, block=block}
end

local function grabLowest(openSet, fScore)
    local lowestScore = nil
    local lowest = nil

    for key, val in pairs(openSet) do
        local score = fScore[key]

        if lowestScore == nil or score < lowestScore then
            lowestScore = score
            lowest = key
        end
    end

    return lowest, lowestScore
end

local function vectorMatches(v1, v2)
    return v1.x == v2.x and v1.y == v2.y and v1.z == v2.z
end

local function getNeighbors(current, scanned)

    local above = vector.new(current.x, current.y + 1, current.z)
    local below = vector.new(current.x, current.y - 1, current.z)

    local neighbors = {}
    local aboveBlock = scanned_at(above.x, above.y, above.z, scanned)
    local belowBlock = scanned_at(below.x, below.y, below.z, scanned)

    if not aboveBlock or aboveBlock.name == "minecraft:air" then
        table.insert(neighbors, above)
    end

    if not belowBlock or belowBlock.name == "minecraft:air" then
        table.insert(neighbors, below)
    end

    for i = 1, 4 do
        local mot = getMotion(i)
        local xMot = mot[1]
        local zMot = mot[2]

        local vec = vector.new(current.x + xMot, current.y, current.z + zMot)
        local block = scanned_at(vec.x, vec.y, vec.z, scanned)
        if not block or block.name == "minecraft:air" then
            table.insert(neighbors, vec)
        end
    end

    return neighbors
end

local function reconstruct_path(cameFrom, current)
    local path = {}
    table.insert(path, current)

    while cameFrom[current] do
        current = cameFrom[current]
        table.insert(path, 1, current)
    end

    return path
end

function aStar(goalVec, blockScanner)

    -- grab starting position
    local startX, startY, startZ, _ = getLocation()
    local start = vector.new(startX, startY - 1, startZ)

    local scanned = blockScanner.scan()
    local openSet = {}
    local cameFrom = {}
    local gScore = {}
    local fScore = {}

    gScore[start] = 0
    fScore[start] = heuristic(start, goalVec)

    openSet[start] = true

    local next = next

    while next(openSet) ~= nil do
        local current, currentFScore = grabLowest(openSet, fScore)

        if vectorMatches(current, goalVec) then
            return reconstruct_path(cameFrom, current)
        end

        openSet[current] = nil
        local neighbors = getNeighbors(current, scanned)

        for _, neighbor in ipairs(neighbors) do
            local tentG = gScore[current] + 1 --turtle can only ever move 1 block, so distance will be 1
            if gScore[neighbor] == nil or tentG < gScore[neighbor] then
                cameFrom[neighbor] = current
                gScore[neighbor] = tentG
                fScore[neighbor] = tentG + heuristic(neighbor, goalVec)
                if openSet[neighbor] == nil then
                    openSet[neighbor] = true
                end
            end
        end
    end

    print("PATH FAILED")
    return {}
end