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

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

local dirWorld = "north"

local file = fs.open("turtle_log", "w")
local blockFile = fs.open("turtle_blocks", "w")

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 logMessage(msg)
	--logMessages[#logMessages + 1] = tostring(msg)
	file.write(tostring(msg) .. "\n")
	file.flush()
	term.write(tostring(msg) .. "\n")
end

local function tableString(tbl)
	local str = "{"
	local size = 0
	for k,v in pairs(tbl) do
		size = size + 1
	end
	local i = 1
	for k,v in pairs(tbl) do
		str = str .. k .. "=" .. v
		if(i ~= size) then str = str .. "," end
		i = i + 1
	end
	str = str .. "}"
	return str
end

-- help
local function round(n)
	return math.floor(n + 0.5)
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
	--logMessage("[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 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()
	local success = turtle.forward()
	if(not success) then
		logMessage("ERROR WHILE MOVING FORWARD: " .. tableString(currentPosition) .. " " .. dirWorld)
	end
	updatePosition()
end

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

	for i, v in pairs(blockQueue) do
		v.x = round(v.x)
		v.y = round(v.y)
		v.z = round(v.z)
		if(v.x == newEntry.x and v.y == newEntry.y and v.z == newEntry.z) 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 rightTurn()
	if(dirWorld == "north") then
		dirWorld = "east"
	elseif(dirWorld == "west") then
		dirWorld = "north"
	elseif(dirWorld == "south") then
		dirWorld = "west"
	elseif(dirWorld == "east") then
		dirWorld = "south"
	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
		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
			blockFile.write(tableString(newEntry) .. "\n")
			blockFile.flush()
		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
		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
			blockFile.write(tableString(newEntry) .. "\n")
			blockFile.flush()
		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
		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
			blockFile.write(tableString(newEntry) .. "\n")
			blockFile.flush()
		end
	end

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

	for key, value in ipairs(blockQueue) do
		if(round(value.x) == round(currentPosition.x) and round(value.y) == round(currentPosition.y) and round(value.z) == round(currentPosition.z)) then
			table.remove(blockQueue, key)
		end
	end
	local newEntry = { x = currentPosition.x, y = currentPosition.y, z = currentPosition.z, name = "air", meta = 0 }
	if(noDuplicate(newEntry)) then
		blockFile.write(tableString(newEntry) .. "\n")
		blockFile.flush()
		blocksExplored[#blocksExplored + 1] = newEntry
		blocksExploredTravellable[#blocksExploredTravellable + 1] = newEntry
	end
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
		value.x = round(value.x)
		value.y = round(value.y)
		value.z = round(value.z)
		ele.x = round(ele.x)
		ele.y = round(ele.y)
		ele.z = round(ele.z)
		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
		value.x = round(value.x)
		value.y = round(value.y)
		value.z = round(value.z)
		current.x = round(current.x)
		current.y = round(current.y)
		current.z = round(current.z)
		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(round(ele.x) == round(key.x) and round(ele.y) == round(key.y) and round(ele.z) == round(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 moveBasedOnDirection(xD, zD)
	--currentPosition.x = currentPosition.x - xD
	--currentPosition.z = currentPosition.z - zD
	if(xD > 0) then
		-- decrease x, west, 101 - 100 = 1
		if(dirWorld == "west") then forward()
		elseif(dirWorld == "north") then leftTurn() turtle.turnLeft() forward()
		elseif(dirWorld == "south") then rightTurn() turtle.turnRight() forward()
		elseif(dirWorld == "east") then leftTurn() leftTurn() turtle.turnLeft() turtle.turnLeft() forward() end
	end
	if(xD < 0) then
		-- increase x, east, 100 - 101 = -1
		if(dirWorld == "east") then forward()
		elseif(dirWorld == "north") then rightTurn() turtle.turnRight() forward()
		elseif(dirWorld == "south") then leftTurn() turtle.turnLeft() forward()
		elseif(dirWorld == "west") then leftTurn() leftTurn() turtle.turnLeft() turtle.turnLeft() forward() end
	end
	if(zD > 0) then
		-- decrease z, north, 101 - 100 = 1
		if(dirWorld == "north") then forward()
		elseif(dirWorld == "west") then rightTurn() turtle.turnRight() forward()
		elseif(dirWorld == "east") then leftTurn() turtle.turnLeft() forward()
		elseif(dirWorld == "south") then leftTurn() leftTurn() turtle.turnLeft() turtle.turnLeft() forward() end
	end
	if(zD < 0) then
		-- increase z, south, 100 - 101 = -1
		if(dirWorld == "south") then forward()
		elseif(dirWorld == "west") then leftTurn() turtle.turnLeft() forward()
		elseif(dirWorld == "east") then rightTurn() turtle.turnRight() forward()
		elseif(dirWorld == "north") then leftTurn() leftTurn() turtle.turnLeft() turtle.turnLeft() forward() end
	end
end

local function moveTo(goal)
	logMessage("From: " .. tableString(currentPosition) .. ", To: " .. tableString(goal))
	local xD = currentPosition.x - goal.x
	local yD = currentPosition.y - goal.y
	local zD = currentPosition.z - goal.z

	local sanityCheck = 0
	if(xD ~= 0) then sanityCheck = sanityCheck + 1 end
	if(yD ~= 0) then sanityCheck = sanityCheck + 1 end
	if(zD ~= 0) then sanityCheck = sanityCheck + 1 end
	if(sanityCheck ~= 1) then
		logMessage("End my existence, " .. sanityCheck)
		logMessage(xD .. " " .. yD .. " " .. zD)
	end
	if(round(yD) > 0) then
		-- move down, 101 - 100 = 1
		turtle.down()
		currentPosition.y = currentPosition.y - yD
	end
	if(round(yD) < 0) then
		-- move up, 100 - 101 = -1
		turtle.up()
		currentPosition.y = currentPosition.y - yD
	end
	moveBasedOnDirection(round(xD), round(zD))
	logMessage("Now: " .. tableString(currentPosition))
end

local function pathfind()
	-- Pathfind our way to the nearest block.
	local nearestBlock = { x = 0, y = 0, z = 0 }
	local nearestBlockValue = 999999
	logMessage("bQ = " .. #blockQueue)
	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
	local path = aSTAR(currentPosition, nearestBlock)

	for key, value in pairs(path) do
		logMessage(tableString(value))
	end

	for key, value in pairs(path) do
		logMessage("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.")