-- [[ API file ]] --
height,width,length = 8,15,16
blocks = textutils.unserialize(fs.open("blocks","r").readAll())

function calculateDistance(tNode1,tNode2)
	local y1 = tNode1[2]
	local z1 = tNode1[3]
	local y2 = tNode2[2]
	local z2 = tNode2[3]
	return math.sqrt( (z2-z1)^2 + (y2-y1)^2 )
	--return math.abs( (z2-z1) + (y2-y1) )
end

function twoOptSwap(route, i, k)
	local new_route = {}
	for c = 1,i-1 do
		table.insert(new_route,route[c])
	end
	for c = k,i,-1 do
		table.insert(new_route,route[c])
	end
	for c = k+1,#route do
		table.insert(new_route,route[c])	
	end
	return new_route
end

function calculateTotalDistance(route)
	local addString = "0 + "
	for i = 1,#route-1 do
		addString = addString..tostring(calculateDistance(route[i],route[i+1])).." + "
	end
	addString = addString.."0"
	loadstring("total_distance = "..addString)()
	return total_distance
end

function getBlockId(x,y,z)
  return blocks[y + z*width + x*length*width + 1]
end

function display(route,distance)
	local l,h = term.getSize()
	shell.run("clear")

	for i = 1,#route-1 do
		paintutils.drawLine(route[i][2],route[i][3],route[i+1][2],route[i+1][3],colors.lime)
	end
	for i,coord in pairs(route) do
		term.setBackgroundColor(colors.yellow)
		term.setTextColor(colors.magenta)
		term.setCursorPos(coord[2],coord[3])
		term.write(string.char(i+64))
	end
	term.setBackgroundColor(colors.black)
	term.setTextColor(colors.white)
	term.setCursorPos(1,h)
	term.write(distance)
end

nodes = {}
local x= 0
for y = 0,width -1 do
	for z = 0,length -1 do
		local id = getBlockId(x,y,z)
		if id and id > 0 then
			table.insert(nodes,{x,y,z})
		end
	end
end

-- [[ Main File ]] --

route = {}
route[1] = {1,28,3}
route[2] = {1,36,13}
route[3] = {1,20,8}
route[4] = {1,8,8}
route[5] = {1,44,5}
route[6] = {1,32,13}
route[7] = {1,20,17}

local existing_route = route
local best_distance = 0

function tsp_algorithm()
	local improve = 0 
	while improve < 20 do
		best_distance = calculateTotalDistance(existing_route)
		for i = 1,#route-1 do
			for k = i + 1, #route do
				new_route = twoOptSwap(existing_route, i, k)
				new_distance = calculateTotalDistance(new_route)
				if new_distance < best_distance then
					improve = 0
					existing_route = new_route
					best_distance = calculateTotalDistance(existing_route)
					display(existing_route,best_distance)
					sleep(.2)
				end
			end
		end
		improve = improve + 1
	end
	return best_distance, existing_route
end

local best_distance,best_route = tsp_algorithm()

--textutils.pagedPrint(textutils.serialize(best_route))