-- Include SaveVariables for data loading
dofile "SaveVariables.lua"

local zones =  {
	-- Weeping coast NB, not sure these are correct, must validate
	[7] = {
		["dx"] = 0.0001027178904108,
		["dy"] = -0.0001040743533489,
		["xo"] = 10843.9424221664,
		["yo"] = 22710.8083349772,
	}
}

function map2world(id,x,y)
	if(not zones[id]) then
		print("Base data for zone not defined: " .. tostring(id))
		return nil
	end
	return (zones[id]["xo"]+(x)/(1000 * zones[id]["dx"])),
		   (zones[id]["yo"]+(y)/(1000 * zones[id]["dy"]));
end

function distance(x1, y1, x2, y2)
	if( x1 == nil or y1 == nil or x2 == nil or y2 == nil ) then
		error("Error: nil value passed to distance()", 2);
	end

	return math.sqrt( (y2-y1)*(y2-y1) + (x2-x1)*(x2-x1) );
end

dozone = 7
aData = yGather_data["db"]
yGather = {}

-- Map yGather to world coordinates for given zone
for zoneId, zoneData in pairs(aData) do
	if (dozone == zoneId) then
		for matId, matData in pairs(zoneData) do
			for _, location in pairs(matData) do
				if ((location ~= nil) and (location[1] ~= nil) and (location[1] >= 0) and (location[1] <= 1000)
						 and (location[2] ~= nil) and (location[2] >= 0) and (location[2] <= 1000)) then
					local wx, wz = map2world(zoneId, location[1], location[2])
					table.insert(yGather, {["X"] = wx, ["Z"] = wz})
				end;
			end;
		end;
	end;
end;

-- Find the yGather points next to the given path
radius=500
waypoints={
	[1] = {	["X"] = 15140, ["Z"] = 15125},
	[2] = {	["X"] = 16361, ["Z"] = 17920},
	[3] = {	["X"] = 18197, ["Z"] = 20188},
}
cur=1
yGatherSorted={}
for idx, waypoint in pairs(waypoints) do
	if (idx < #waypoints) then
		--print(string.format("Working from point [%d, %d)", waypoints[idx].X, waypoints[idx].Z))
		local l = distance(waypoints[idx+1].X, waypoints[idx+1].Z, waypoints[idx].X, waypoints[idx].Z)
		
		local tmp = {}
		for i,v in pairs(yGather) do
			local a = distance(v.X, v.Z, waypoints[idx].X, waypoints[idx].Z)
			local b = distance(waypoints[idx+1].X, waypoints[idx+1].Z, v.X, v.Z)
			local q = (((a^2)-(b^2))/(2*(l^2)))+0.5
			local p = 1 - q
			
			if ((math.abs(q) < 1) and (math.abs(p) < 1)) then
				local d = math.sqrt((a^2) - ((q*l)^2))
				if (math.abs(d) < radius) then
					table.insert(tmp, v)
				end
			end
		end
		
		-- Insert current waypoint
		table.insert(yGatherSorted, waypoint)
		
		-- Sort the yGather points by distance 
		cur=1
		while (#tmp>0) do
			v = table.remove(tmp, cur)

			local closest = 1;
			v.d = math.huge
			for i,w in pairs(tmp) do
				local d = distance(v.X, v.Z, w.X, w.Z)
				if (d <= v.d) then
					closest = i;
					v.d = d;
				end
			end
			if (v.d == math.huge) then
				v.d = 0
			end
			table.insert(yGatherSorted, v)
			cur = closest;
		end
	end
end
yGather=yGatherSorted
print()


--print("<waypoints>")
for i,v in pairs(yGather) do
	--print(string.format("    <!-- distance to next = %5d --><waypoint x=\"%d\" z=\"%d\"></waypoint>", v.d, v.X, v.Z))
	print(string.format("%d, %d", v.X, v.Z))
end
--print("</waypoints")