local startingPosition = { x = 0, y = 0, z = 0}

local blocksExplored = {}
local blocksExploredTravellable = {}
local blockQueue = {}
local logMessages = {}

local dirWorld = "north"

term.write("Enable debug output mode?: ")
local debugOn = true
if(io.read("*l") == "false") then
	debugOn = false
end
term.write("Enable block logging mode?: ")
local blockLog = true
if(io.read("*l") == "false") then
	blockLog = false
end
term.write("Enable distance logging mode?: ")
local distanceLog = true
if(io.read("*l") == "false") then
	distanceLog = false
end
term.write("Enter turtle's x coordinate: ")
startingPosition.x = io.read("*l")
term.write("Enter turtle's y coordinate: ")
startingPosition.y = io.read("*l")
term.write("Enter turtle's z coordinate: ")
startingPosition.z = io.read("*l")
local currentPosition = startingPosition

local function tableString(tbl)
	local str = "{"
	for k,v in pairs(tbl) do
		str = str .. k .. "=" .. v
	end
	str = str .. "}"
	return str
end

local function consumeFuel()
	for i = 1, 16, 1 do
		local fuelDifference = turtle.getFuelLimit() - turtle.getFuelLevel()
		if(fuelDifference < (80 * 64)) then
			turtle.refuel()
		end
	end
	term.write("[CaveEX]: New fuel amount is " .. turtle.getFuelLevel() .. " actions/moves.")
end

local function compactPosition()
	return "x: " .. currentPosition.x .. ", y: " .. currentPosition.y .. ", z: " .. currentPosition.z
end

local function logMessage(msg)
	logMessages[#logMessages + 1] = tostring(msg)
	if(debugOn) then print(msg) end
end

local function updatePosition()
	if(dirWorld == "north") then
		startingPosition.z = startingPosition.z + 1
	elseif(dirWorld == "south") then
		startingPosition.z = startingPosition.z - 1
	elseif(dirWorld == "west") then
		startingPosition.x = startingPosition.x - 1
	elseif(dirWorld == "east") then
		startingPosition.x = startingPosition.x + 1
	end
end

local function forward(n)
	for i = 1, n, 1 do
		turtle.forward()
		updatePosition()
	end
end

local function noDuplicate(newEntry)
	for i, v in pairs(blocksExplored) do
		if(v == newEntry) then
			logMessage("[CaveEX]: Found duplicate in blocksExplored " .. tableString(newEntry))
			return false
		end
	end

	for i, v in pairs(blockQueue) do
		if(v == newEntry) then
			logMessage("[CaveEX]: Found duplicate in blockQueue " .. tableString(newEntry))
			return false
		end
	end

	return true
end

local function dirToOffset()
	if(dirWorld == "north") then
		return 0, 1
	elseif(dirWorld == "south") then
		return 0, -1
	elseif(dirWorld == "west") then
		return -1, 0
	elseif(dirWorld == "east") then
		return 1, 0
	end
end

local function leftTurn()
	if(dirWorld == "north") then
		dirWorld = "west"
	elseif(dirWorld == "west") then
		dirWorld = "south"
	elseif(dirWorld == "south") then
		dirWorld = "east"
	elseif(dirWorld == "east") then
		dirWorld = "north"
	end
end

local function addIfAirFront()
	consumeFuel()
	if(not turtle.detect()) then
		local x0, z0 = dirToOffset()
		local newEntry = { x = currentPosition.x + x0, y = currentPosition.y, z = currentPosition.z + z0 }
		if(noDuplicate(newEntry)) then
			blockQueue[#blockQueue + 1] = newEntry
			if(blockLog) then logMessage("[CaveEX]: blockQueue += " .. tableString(blockQueue[#blockQueue])) end
		end
	else
		local x0, z0 = dirToOffset()
		local success, data = turtle.inspect()
		local newEntry = { x = currentPosition.x + x0, y = currentPosition.y, z = currentPosition.z + z0, name = data.name, meta = data.metadata }
		if(noDuplicate(newEntry)) then
			blocksExplored[#blocksExplored + 1] = newEntry
			if(blockLog) then logMessage("[CaveEX]: blocksExplored += " .. tableString(blocksExplored[#blocksExplored])) end
		end
	end
	turtle.turnLeft()
	leftTurn()
end

local function addIfAir()
	if(not turtle.detectDown()) then
		local newEntry = { x = currentPosition.x, y = currentPosition.y - 1, z = currentPosition.z }
		if(noDuplicate(newEntry)) then
			blockQueue[#blockQueue + 1] = newEntry
			if(blockLog) then logMessage("[CaveEX]: blockQueue += " .. tableString(blockQueue[#blockQueue])) end
		end
	else
		local success, data = turtle.inspectDown()
		local newEntry = { x = currentPosition.x, y = currentPosition.y - 1, z = currentPosition.z, name = data.name, meta = data.metadata }
		if(noDuplicate(newEntry)) then
			blocksExplored[#blocksExplored + 1] = newEntry
			if(blockLog) then logMessage("[CaveEX]: blocksExplored += " .. tableString(blocksExplored[#blocksExplored])) end
		end
	end

	if(not turtle.detectUp()) then
		local newEntry = { x = currentPosition.x, y = currentPosition.y + 1, z = currentPosition.z }
		if(noDuplicate(newEntry)) then
			blockQueue[#blockQueue + 1] = newEntry
			if(blockLog) then logMessage("[CaveEX]: blockQueue += " .. tableString(blockQueue[#blockQueue])) end
		end
	else
		local success, data = turtle.inspectUp()
		local newEntry = { x = currentPosition.x, y = currentPosition.y + 1, z = currentPosition.z, name = data.name, meta = data.metadata }
		if(noDuplicate(newEntry)) then
			blocksExplored[#blocksExplored + 1] = newEntry
			if(blockLog) then logMessage("[CaveEX]: blocksExplored += " .. tableString(blocksExplored[#blocksExplored])) end
		end
	end

	addIfAirFront()
	addIfAirFront()
	addIfAirFront()
	addIfAirFront()

	blocksExplored[#blocksExplored + 1] = { x = currentPosition.x, y = currentPosition.y, z = currentPosition.z, name = "air", meta = "nil" }
	blocksExploredTravellable[#blocksExploredTravellable + 1] = { x = currentPosition.x, y = currentPosition.y, z = currentPosition.z }
end

term.write("CaveEX starting from " .. compactPosition())

local timeStart = os.clock()

consumeFuel()

-- Populate blockQueue with the blocks around us.

addIfAir()

local function lowestTable(tbl)
	local lowestValue = 999999
	local lowest = nil
	for key, value in pairs(tbl) do
		if(value < lowestValue) then
			lowestValue = value
			lowest = key
		end
	end
	return lowest
end

local function indexTable(tbl, ele)
	for key, value in pairs(tbl) do
		if(value.x == ele.x and value.y == ele.y and value.z == ele.z) then
			return key
		end
	end
end

local function getNeighbours(current)
	local returnable = {}
	for key, value in pairs(blocksExploredTravellable) do
		if(value.x == current.x and value.y == current.y and (value.z == current.z + 1 or value.z == current.z - 1)) then
			returnable[#returnable + 1] = value
		elseif((value.x == current.x + 1 or value.x == current.x - 1) and value.y == current.y and value.z == current.z) then
			returnable[#returnable + 1] = value
		elseif(value.x == current.x and (value.y == current.y + 1 or value.y == current.y - 1) and value.z == current.z) then
			returnable[#returnable + 1] = value
		end
	end
end

local function h(value, goal)
	local xD = goal.x - value.x
	local yD = (goal.y - value.y) * 10
	local zD = goal.z - value.z
	return math.sqrt(xD^2 + yD^2 + zD^2)
end

local function tableIn(tbl, ele)
	for key, value in pairs(tbl) do
		if(ele.x == key.x and ele.y == key.y and ele.z == key.z) then
			return true
		end
	end

	return false
end

local function tablePrepend(tbl, ele)
	local returnable = { ele }

	for key, value in pairs(tbl) do
		returnable[key] = value
	end

	return returnable
end

local function reconstructPath(cameFrom, current)
	totalPath = { current }
	while(tableIn(cameFrom, current)) do
		current = cameFrom[current]
		totalPath = tablePrepend(totalPath, current)
	end
	return totalPath
end

local function aSTAR(start, goal)
	local openSet = { start } -- number indexed
	local cameFrom = {} -- value indexed

	local gScore = {} -- value indexed
	gScore[start] = 0

	local fScore = {} -- value indexed
	fScore[start] = h(start, goal)

	while(#openSet > 0) do
		local current = lowestTable(fScore) -- check
		if(current == goal) then
			--term.write("I FRIKKIN GOT IT BOY!")
			return reconstructPath(cameFrom, current)
		end

		openSet[indexTable(openSet, current)] = nil -- check
		if(getNeighbours(current)) then
			for key, value in pairs(getNeighbours(current)) do
				local tentative_gScore = gScore[current] + 1
				if(tentative_gScore < gScore[value]) then
					cameFrom[value] = current
					gScore[value] = tentative_gScore
					fScore[value] = gScore[value] + h(value, goal)
					if(openSet[indexTable(openSet, value)] == nil) then
						openSet[#openSet + 1] = value
					end
				end
			end
		else
			--term.write("No neighbors...")
			return { goal }
		end
	end

	--term.write("didn't find it, sad!")
end

local function moveTo(goal)
	print("From: " .. tableString(currentPosition) .. ", To: " .. tableString(goal))
end

local function pathfind()
	-- Pathfind our way to the nearest block.
	local nearestBlock = { x = 0, y = 0, z = 0 }
	local nearestBlockValue = 999999
	for _,v in pairs(blockQueue) do
		local xD = currentPosition.x - v.x
		local yD = (currentPosition.y - v.y) * 10 -- height difference mega important
		local zD = currentPosition.z - v.z
		local distance = math.sqrt(xD^2 + yD^2 + zD^2)
		if(distanceLog) then logMessage("[CaveEX]: New distance " .. distance) end
		if(distance < nearestBlockValue) then
			nearestBlockValue = distance
			nearestBlock = { x = v.x, y = v.y, z = v.z }
		end
	end
	if(distanceLog) then logMessage("[CaveEX]: Closest block " .. tableString(nearestBlock)) end
	local path = aSTAR(currentPosition, nearestBlock)

	for key, value in pairs(path) do
		print("Moving to: " .. key .. " = " .. tableString(value))
		moveTo(value)
	end
end

--[[while(#blockQueue > 0) do
	pathfind()
	addIfAir()
end]]

pathfind()

local timeEnd = os.clock() - timeStart

term.write("CaveEX finished in " .. timeEnd .. " seconds.")