local gpsConfig = "config/gps"
local currentF = 0
local currentX = 0
local currentY = 0
local currentZ = 0
local gpsConfig = {}
local FileVersion = "Alpha 1.0"

function gotoPos(targetX,targetY,targetZ,targetF)
	print("Loading GPS-Data")
	Position = filesystem.loadFile("config/gps")
	if Position == nil then
		error("GPS-config Datei nicht gefunden: "..gpsConfig)
	else
		currentF = tonumber(Position["f"])
		currentX = tonumber(Position["x"])
		currentY = tonumber(Position["y"])
		currentZ = tonumber(Position["z"])
			print("loaded Position")
			print(currentX.." "..currentY.." "..currentZ)
		travelHigh = tonumber(Position["travelHigh"]
			print("loaded TravelHigh "..travelHigh)
			print("Going to Road")
		goRoad(currentY)
			print("Finished")
			print("Fixing X-Coord")
		goX(targetX)
			print("Finished")
			print("Fixing Z-Coord")
		goZ(targetZ)
			print("Finished")
			print("Facing the right direction")
		faceTo(targetF)
			print("Finished")
			print("Going down to right Position")
		goY(targetY)
			print("Ziel erreicht")
	end
	saveGPS()
end

function getPos()
	aktPos = filesystem.loadFile(config/gps)
	return aktPos
end

function faceTo(direction)
	print("FaceTo "..direction)
	while currentF ~= direction do
		right()
	end
end

function goX(targetX)
	if targetX > currentX then
		faceTo(3)
		while targetX ~= currentX do
			print("  X-not in POS")
			print("    must go EAST")
			forward()
		end
	elseif targetX < currentX then
		faceTo(1)
		while targetX ~= currentX do
			print("  X-not in POS")
			print("    must go WEST")
			forward()
		end
	end
end

function goY(targetY)
	while currentY ~= targetY do
		print("  Y-not in POS")
		if currentY > targetY then
			print("    must go down")
			down()
		elseif currentY < targetY then
			print("    must go up")
			up()
		end
	end
end

function goZ(targetZ)
	if targetZ > currentZ then
		faceTo(0)
		while currentZ ~= targetZ do
			print("  Z-not in POS")
			print("    must go SOUTH")
			forward()
		end
	elseif targetZ < currentZ then
		faceTo(2)
		while currentZ ~= targetZ do
			print("  Z-not in POS")
			print("    must go NORTH")
			forward()
		end
	end
end

function up()
	moved = false
	while not(moved) do
		moved = turtle.up()
	end
	currentY = currentY + 1
	saveGPS()
end

function down()
	moved = false
	while not(moved) do
		moved = turtle.down()
	end
	currentY = currentY - 1
	saveGPS()
end

function left()
	turtle.turnLeft()
	currentF = currentF - 1
	if currentF < 0 then
		currentF = 3
	end
	saveGPS()
end

function right()
	turtle.turnRight()
	currentF = currentF + 1
	if currentF > 3 then
		currentF = 0
	end
	saveGPS()
end

function forward()
	moved = false
	if currentF == 0 then
		currentZ = currentZ + 1
		while not(moved) do
			moved = turtle.forward()
		end
	elseif currentF == 1 then
		currentX = currentX - 1
		while not(moved) do
			moved = turtle.forward()
		end
	elseif currentF == 2 then
		currentZ = currentZ - 1
		while not(moved) do
			moved = turtle.forward()
		end
	elseif currentF == 3 then
		currentX = currentX + 1
		while not(moved) do
			moved = turtle.forward()
		end
	end
	saveGPS()
end

function back()
	moved = false
	if currentF == 0 then
		currentZ = currentZ - 1
		while not(moved) do
			moved = turtle.back()
		end
	elseif currentF == 1 then
		currentX = currentX + 1
		while not(moved) do
			moved = turtle.back()
		end
	elseif currentF == 2 then
		currentZ = currentZ + 1
		while not(moved) do
			moved = turtle.back()
		end
	elseif currentF == 3 then
		currentX = currentX - 1
		while not(moved) do
			moved = turtle.back()
		end
	end
	saveGPS()
end

function saveGPS()
	gpsConfig["x"] = tonumber(currentX)
	gpsConfig["y"] = tonumber(currentY)
	gpsConfig["z"] = tonumber(currentZ)
	gpsConfig["f"] = tonumber(currentF)
	gpsConfig["travelHigh"] = tonumber(travelHigh)
	filesystem.writeFile("config/gps",gpsConfig)
	print(" X= "..currentX.." Y= "..currentY.." Z= "..currentZ)
end


function goRoad()
	print("move from current High "..currentY)
	print("to travelHigh "..travelHigh)
	while currentY ~= travelHigh do
		if currentY > travelHigh then
			down()
		elseif currentY < travelHigh then
			up()
		end
	end
end

function init()
	print("Loading GPS-Data")
	Position = filesystem.loadFile("config/gps")
	if Position == nil then
		error("GPS-config Datei nicht gefunden: "..gpsConfig)
	else
		currentF = tonumber(Position["f"])
		currentX = tonumber(Position["x"])
		currentY = tonumber(Position["y"])
		currentZ = tonumber(Position["z"])
			print("loaded Position")
			print(currentX.." "..currentY.." "..currentZ)
		travelHigh = tonumber(Position["travelHigh"]
			print("loaded TravelHigh "..travelHigh)
	end
end

function loadWaypoint(filename)
	Waypoint = filesystem.loadFile("waypoints/"..filename)
	if Waypoint == nil then
		error("Datei nicht gefunden: "..filename)
	else
		return Waypoint
	end
end

function listWaypoints()
	WaypointList = fs.list("waypoints/")
	return WaypointList
end

function createWaypoint(Waypoint)
	filesystem.writeFile("waypoints/"..Waypoint["name"],Waypoint)
end

function deleteWaypoint(Waypoint)
	filesystem.delete(Waypoint)
end