SuperStrict

Graphics 1280, 720

Const rasterSize:Int = 256

Global land:TLandscape = New TLandscape
Local timer:TTimer = CreateTimer(60)
Local unit:TUnit = Tunit.Create(1, 1, land.map)

Global zoom:Float = 1.0
Global aStern_dauer:Int = 0
Global aStern_aufrufe:Int = 0
Global _fps:Int = 0
Global _fpsOld:Int = 0
Global _fpsTimer:Int = MilliSecs()
Global nodesPerFrame:Int = 100

Global off:TList = CreateList()
Global clos:TList = CreateList()

Global _timer2:TTimer = CreateTimer(1000)



While Not KeyHit(KEY_ESCAPE) And Not AppTerminate()
	Cls
	
	nodesPerFrame = 100
	
	Local oriX:Float = 0
	Local oriY:Float = 0
	
	GetOrigin(oriX, oriY)
	oriX:+KeyDown(KEY_A) * 3.5 - KeyDown(KEY_D) * 3.5
	oriY:+KeyDown(KEY_W) * 3.5 - KeyDown(KEY_S) * 3.5
	SetOrigin(oriX, oriY)
	
	Local mx:Int = VirtualMouseX() - oriX
	Local my:Int = VirtualMouseY() - oriY
	
	If MouseHit(1) Then
	'	unit.AStern = TAStern2D.Create(unit.posx, unit.posy, (mx) / 10, (my) / 10, land.map, 1)
		TUnit.Create(mx / 10, my / 10, land.map)
	ElseIf MouseHit(3) Then
		Local list:TList = CreateList()
		Local x:Int = 0
		Local y:Int = 0
		
		For Local u:TUnit = EachIn TUnit.list
			list.AddLast(u)
			x:+u.posx
			y:+u.posy
		Next
		x:/list.count()
		y:/list.count()
		TUnit.multiTarget(list, TAStern2D.Create(x, y, mx / 10, my / 10, land.map))
	EndIf
	If MouseDown(2) Then
		land.setBlock(mx / 10, my / 10, 1 - KeyDown(KEY_LCONTROL))
	End If
	
	If KeyHit(KEY_ENTER) Then
		SetVirtualResolution(1280, 720)
	End If
	Local mwS:Int = MouseZSpeed()
	If mwS <> 0 Then
		zoom:+(mwS / 100.0)
		SetVirtualResolution(1280 * zoom, 1280 * zoom * 0.5625)
	End If
	
	TUnit.Update()
	
	land.draw()
	SetColor(255, 255, 255)
	DrawText("Dauer: " + aStern_dauer, -oriX + 50, -oriY + 50)
	DrawText("FPS: " + _fpsOld, -oriX + 50, -oriY + 65)
	DrawText("Aufrufe: " + aStern_aufrufe, -oriX + 150, -oriY + 50)


	Flip 0
	
	WaitTimer(timer)
	
	_fps:+1
	
	If _fpsTimer < MilliSecs() Then
		_fpsOld = _fps
		_fps = 0
		_fpsTimer = MilliSecs() + 1000
	End If
Wend

Const sekSize:Int = 16
Const sekTileCnt:Int = (rasterSize / sekSize)
Type TLandscape

	
	Field map:Short[][]
	Field sektors:TSektor[,]
	
	Field waypoints:TWaypoint[]
	
	Method New()
		Self.map = Self.map[..rasterSize]
		For Local i:Int = 0 Until rasterSize
			Self.map[i] = New Short[rasterSize]
			Self.map[i][0] = 1
			Self.map[i][rasterSize - 1] = 1
		Next
		For Local a:Int = 0 Until rasterSize
			Self.map[0][a] = 1
			Self.map[rasterSize - 1][a] = 1
		Next
		
		Self.sektors = New TSektor[sekTileCnt, sekTileCnt]
		
		For Local x:Int = 0 Until (sekTileCnt)
		For Local y:Int = 0 Until (sekTileCnt)
			Self.sektors[x, y] = New TSektor
		Next
		Next
		
		'in der mitte eines jeden sektors einen Wegpunkt erzeugen!
	End Method
	Method CheckWaypointPossible:Int(x:Int, y:Int)
		'Schritt 1: Sektor bestimmen.
		Local sekX:Int = x / sektileCnt
		Local sekY:Int = y / sektilecnt
		
		'Schritt 2: Alle Sektoren im umfeld abfragen und die Wegpunkte-indices sammeln.
		Local wpIndices:Int[] = Self.sektors[sekx, seky].waypoints[..]
		Local x2:Int, y2:Int
		
		For x2 = x - 1 To x + 1
		For y2 = y - 1 To y + 1
			If x2 = x And y2 = y Then Continue
			If x2 < 0 Or y2 < 0 Or y2 >= sekSize Or x2 >= sekSize Then Continue
			
			wpIndices:+Self.sektors[x2, y2].waypoints
		Next
		Next
		
		'Schritt 3: Die distanzen überprüfen.
		Local dist:Int = 20, dist2:Int
		Local index:Int = -1
		For Local i:Int = 0 Until Len(wpIndices)
			If Self.waypoints[i] Then
				dist2 = Abs(x - Self.waypoints[i].x) + Abs(y - Self.waypoints[i].y)
				If dist2 < dist Then
					index = i
					dist = dist2
					If dist < 8 Then Exit 'der wegpunkt ist zu nah! exit.
				End If
			End If
		Next
		
		If index > - 1 Then
			'ein wegpunkt wurde gefunden!
			Local as:TAstern2d = TAstern2d.Create(x, y, Self.waypoints[index].x, Self.waypoints[index].y, Self.map)
			If as.Update(20) Then
				'weg vorhanden - nicht einfügen.
				Return Null
			Else
				
			End If
		End If
	End Method
	Method draw()
		
		Local x:Int, y:Int
		Local sizeY:Int
		SetColor(200, 200, 200)
		For x = 0 Until Len(map)
			sizeY = Len(map[x])
			For y = 0 Until sizeY
			
				If map[x][y] = 1 Then
					DrawRect(x * 10 - 5, y * 10 - 5, 10, 10)
				End If
			
			Next
		Next
	End Method
	
	Method SetBlock:Int(x:Int, y:Int, id:Int)
		If x < 0 Or y < 0 Then Return - 1
		
		If x >= Len(map) Or y >= Len(map[x]) Then Return - 1
		
		map[x][y] = id
	End Method
End Type

Type TWaypoint
	Field x:Int
	Field y:Int
	Field index:Int
	
	Field neibours:Int[]
End Type
Type TSektor
	Field waypoints:Int[]
End Type

Type TUnit
	Global list:TList = CreateList()
	
	Field posx:Int
	Field posy:Int
	
	Field _X:Float
	Field _Y:Float
	
	Field aStern:TAStern2d
	
	Field waypoints:Int[][]
	
	Field _multiTarget:Byte = 0 '1, wenn ein Multitarget bestimmt wurde-> heißt, bis zum startpunkt einen eigenen aStern vorhängen!
	Field tmpWaypoints:Int[][]
	
	Function Create:TUnit(posx:Int, posy:Int, mapArr:Short[][])
		If posx < 0 Or posy < 0 Or posx >= Len(mapArr) Or posy >= Len(mapArr[0]) Then Return Null
		
		If mapArr[posx][posy] = 1 Then Return Null
		
		Local unit:TUnit = New TUnit
		
		unit.posx = posx
		unit.posy = posy
		
		TUnit.list.AddLast(unit)
		
		Return unit
	End Function
	Function MultiTarget:Int(Unit_list:TList, aStar:TAstern2d)
		For Local u:TUnit = EachIn Unit_list
			u.aStern = aStar
			u._multiTarget = 1
			u.waypoints = Null
		Next
	End Function
	Function Update:Int()
		SetColor(255, 0, 0)
		For Local unit:TUnit = EachIn TUnit.list
			unit._update()
			unit._draw()
		Next
	End Function
	
	Method _Update:Int()
		If aStern Then
			'Local ms:Int = MilliSecs()
			If nodesPerFrame > 0 Then
				Local times:Int
				If nodesPerFrame > 200 Then
					times = 200
				Else
					times = nodesPerFrame
				End If
				
				Local waypoints:Int[][] = AStern.Update(times)
				nodesPerFrame:-aStern.aufrufe
				aStern.aufrufe = 0
				'Print "Dauer: " + (MilliSecs() - ms)
				If Not waypoints Then
					If AStern.impossible Then
						Print "Astern:: Impossible"
						Self.AStern = Null
					End If
				Else
				'	Print "Astern:: Erfolgreich - eingetragen!"
				'	aStern_dauer = MilliSecs() - Self.aStern.dauer
				'	aStern_aufrufe = Self.aStern.aufrufe
					Select Self._multiTarget
						Case 0
							Self.waypoints = waypoints
							Self.aStern = Null
						Case 1
							Self.tmpWaypoints = waypoints[..Len(waypoints)] 'kopieren!
							Local x:Int, y:Int
							Local dist:Int = 1000000000
							Local dist2:Int
							For Local i:Int = 0 Until Len(Self.tmpWaypoints)
								If Self.tmpWaypoints[i] Then
									dist2 = Abs(Self.posx - Self.tmpWaypoints[i][0]) + Abs(Self.posy - Self.tmpWaypoints[i][1])
									If dist2 < dist Then
										dist= dist2
										x = Self.tmpWaypoints[i][0]
										y = Self.tmpWaypoints[i][1]
									EndIf
								End If
							Next
							Self.aStern = TAStern2D.Create(Self.posx, Self.posy, x, y, land.map)
							Self._multiTarget = 2
						Case 2
							Self.tmpWaypoints = waypoints + Self.tmpWaypoints
							Self.waypoints = Self.tmpWaypoints
							Self.tmpWaypoints = Null
							Self.aStern = Null
							Self._multiTarget = 0
					End Select
				End If
			EndIf
		EndIf
		
		'wegpunkte abgehen.
		Local i:Int
		Local wp:Int[]
		If Not Self.waypoints Then
			If Not Self.aStern Then Self.aStern = TAStern2D.Create(Self.posx, Self.posy, Rand(0, rasterSize - 1), Rand(0, rasterSize - 1), land.map)
			Return 0
		EndIf
		For i = 0 Until Len(Self.waypoints)
			If Self.waypoints[i] Then
				wp = Self.waypoints[i]
				Exit
			End If
		Next
		
		If wp Then
			Local dirX:Int = Sgn(Self.posx - wp[0])
			Local dirY:Int = Sgn(Self.posy - wp[1])
			
			Self._X:-dirX / 1.0
			Self._Y:-dirY / 1.0
			
			If Self._X <= - 10 Then
				Self.posx:-1
				Self._X = 0
			ElseIf Self._X >= 10 Then
				Self.posx:+1
				Self._X = 0
			EndIf
			If Self._Y <= - 10 Then
				Self.posy:-1
				Self._Y = 0
			ElseIf Self._Y >= 10 Then
				Self.posy:+1
				Self._Y = 0
			EndIf
			
			If Self.posx = wp[0] And Self.posy = wp[1] Then
				Self.waypoints[i] = Null
			End If
		Else
			'keine WPs gefunden -> WP löschen.
			Self.waypoints = Null
		End If
	End Method
	
	Method _draw()
		DrawOval(posx * 10 - 5 + _X, posy * 10 - 5 + _Y, 10, 10)
		
		If Self.waypoints Then
			Local i:Int = 0
			Local wp:Int[]
			For i = Len(Self.waypoints) - 1 To 0 Step - 1
				If Self.waypoints[i] Then
					wp = Self.waypoints[i]
					Exit
				End If
			Next
			
			If wp Then
				DrawLine(posx * 10 + _X, posy * 10 + _Y, wp[0] * 10, wp[1] * 10)
			End If
		Else
			SetColor(255, 180, 180)
			DrawText("?", posx * 10 + 10, posy * 10 + 10)
			SetColor(255, 0, 0)
		End If
	End Method
End Type


Type TAStern2D
	Global list:TList = CreateList()
	
	Field _offen:TPriorityHeap = New TPriorityHeap
	Field _geschlossen:Byte[,]
	Field __offenArr:TKnoten[,]
	
	Field zielX:Int
	Field zielY:Int
	
	Field map:Short[][]
	Field sizeX:Int
	Field sizeY:Int
	
	Field impossible:Byte = 0
	
	Field dauer:Int = 0
	Field aufrufe:Int = 0
	
	Field isReady:Int[][] ' ist <> null, wenn bereits fertig!
	
	Function Create:TAStern2D(startX:Int, startY:Int, zielX:Int, zielY:Int, mapArray:Short[][], attach_to_list:Byte = False)
		Local as:TAStern2D = New TAStern2D
		If zielX < 0 Or zielY < 0 Or zielX >= rasterSize Or zielY >= rasterSize Then Return Null
		
		
		as.zielX = zielX
		as.zielY = zielY
		
		as.map = mapArray
		
		as.sizeX = Len(mapArray)
		For Local i:Int = 0 Until as.sizeX
			'findet einen brauchbaren punkt - geht von einem "quadratischen" array aus!
			If mapArray[i] Then
				as.sizeY = Len(mapArray[i])
				Exit
			End If
		Next
		
		If mapArray[zielX][zielY] > 0 Then Return Null
		
		as._geschlossen = New Byte[as.sizeX, as.sizeY]
		as.__offenArr = New TKnoten[as.sizeX, as.sizeY]
		Local knoten:Tknoten = TKnoten.Create(startx, starty, zielx, ziely, Null, 0)
		
		as._offen.add(knoten.f_costs, knoten)
	'	Print "Startnode eingefügt"
		If attach_to_list Then TAStern2D.list.AddLast(as)
		
		as.dauer = MilliSecs()
		
		off.Clear()
		clos.Clear()
		Return as
		
	End Function
	
	Method Update:Int[][] (times:Int)
		If Self.isReady Then Return Self.isReady
		
		Local knoten:TKnoten
		Local x:Int, y:Int
		Local step_x:Int, step_y:Int
		Local newNode:TKnoten
	
		'Geht TIMES x die knoten ab. Dh es werden TIMES knoten bearbeitet.
		For Local i:Int = 0 Until times
			Self.aufrufe:+1
			'holt den Besten knoten.
			
			knoten = Null
			x = 0
			y = 0
			step_x = 0
			step_y = 0
			newNode = Null
			
			If Self._offen.isEmpty() Then
				TAStern2D.list.Remove(Self)
				Self.impossible = 1
				Return Null
			End If
			knoten = TKnoten(Self._offen.removeHighest().data())
			
			off.Remove(knoten)
			clos.AddLast(knoten)
			
			Self._geschlossen[knoten.posx, knoten.posy] = 1 'in GESCHLOSSEN eintragen. 
			Self.__offenArr[knoten.posx, knoten.posy] = Null 'aus OFFEN entfernen.
			
			'position setzen
			x = knoten.posx
			y = knoten.posy
			
			If x = zielX And y = zielY Then
				Self.isReady = Self.BuildWaypoint(knoten)
				Return Self.isReady
			End If
			
			'umliegende punkte überprüfen.
			For step_x = x - 1 To x + 1
			For step_y = y - 1 To y + 1
				If step_x = x And step_y = y Then Continue
				'Print "bla"
				'out of bounds: Continue.
				If step_x < 0 Or step_y < 0 Or step_x >= sizeX Or step_y >= sizeY Then
				'	Print "X: " + step_x + " Y: " + step_y + " - Out of bounds"
					Continue
				EndIf
				'Wallcheck
				If Self.map[step_x][step_y] > 0 Then
				'	Print "X: " + step_x + " Y: " + step_y + " - Wand"
					Continue
				EndIf 'wand.
				
				'geschlossen: Continue.
				If _geschlossen[step_x, step_y] Then
				'	Print "X: " + step_x + " Y: " + step_y + " - geschlossen"
					Continue
				EndIf
				
				Local g:Int = 14 - ((x = step_x Or y = step_y) * 4) '10 für Graden schritt, 14 für einen Schrägen schritt. Die abfrage ergibt 0, wenn diagonal.
				
				'Check, obs ein diagonaler schritt an einer wand lang wäre!
				If g = 14 Then
					Local dirX:Int = Sgn(step_x - x)
					Local dirY:Int = Sgn(step_y - y)
					
					If dirX = 1 And step_x > 0 And map[step_x - 1][step_y] > 0 Then
						Continue
					ElseIf dirX = -1 And step_x < sizeX - 1 And map[step_x + 1][step_y] > 0 Then
						Continue
					ElseIf dirY = 1 And step_y > 0 And map[step_x][step_y - 1] > 0 Then
						Continue
					ElseIf dirY = -1 And step_y < sizeY - 1 And map[step_x][step_y + 1] > 0 Then
						Continue
					EndIf
				End If
				
				g:+knoten.g_costs
				
				'Check: Ist in offen?
				If __offenArr[step_x, step_y] Then
					'knoten ist offen. G vergleich.
					If __offenArr[step_x, step_y].g_costs > g Then
						'umschreiben.
						__offenArr[step_x, step_y].ReLink(knoten, g)
					End If
					Continue
				End If
				
				'nicht geschlossen, nicht offen, nicht out of bounds: einfügen.
				newNode = TKnoten.Create(step_x, step_y, zielX, zielY, knoten, g)
				__offenArr[step_x, step_y] = newNode
				_offen.add(newNode.f_costs, newNode)
				off.AddLast(newNode)
				
			'	Print "node eingefügt."
			Next
			Next
			
			If KeyDown(KEY_ENTER) DrawStep()
		Next
	End Method
	
	Method BuildWaypoint:Int[][] (knoten:TKnoten)
		Local ret:Int[][]
		
		ret = ret[..Int(knoten.g_costs / 10) + 1]
		
		Local i:Int = Len(ret) - 1
		Local a:Int = 0
		While knoten
			ret[i] = New Int[2]
			
			ret[i][0] = knoten.posx
			ret[i][1] = knoten.posy
			
			i:-1
			a:+1
			
			knoten = knoten._prev
		
		Wend
	'	Print "len vorher: " + Len(ret)
	'	Print "A: " + a + " i: " + i + " len nach slicen: " + Len(ret[i..])
		Return ret'[i..]
	End Method
End Type

Type TKnoten
	Field posx:Int
	Field posy:Int
	
	Field f_costs:Int
	Field h_costs:Int
	Field g_costs:Int
	
	Field _prev:TKnoten
	
	Function Create:TKnoten(posx:Int, posy:Int, zielX:Int, zielY:Int, previous_node:TKnoten, g_costs:Int)
		Local knoten:TKnoten = New TKnoten
		
		knoten.posx = posx
		knoten.posy = posy
		
		Local h:Int = 14 * Max(Abs(posx - zielX), Abs(posy - zielY))
		
		knoten.h_costs = h
		knoten.g_costs = g_costs
		knoten.f_costs = g_costs + knoten.h_costs
		
		knoten._prev = previous_node
		
		Return knoten
	End Function
	
	Method Relink(knoten:TKnoten, g_costs:Int)
		Self._prev = knoten
		Self.g_costs = g_costs
		Self.f_costs = Self.h_costs + Self.g_costs
	End Method
End Type

Function DrawStep()
	Cls
	
	SetColor(255, 180, 180)
	For Local kno:TKnoten = EachIn clos
		DrawRect(kno.posx * 10 - 4, kno.posy * 10 - 4, 8, 8)
	Next
	SetColor(180, 255, 180)
	For Local bla:TKnoten = EachIn off
		DrawRect(bla.posx * 10 - 4, bla.posy * 10 - 4, 8, 8)
	Next
	
	Flip 1
	
	WaitTimer(_timer2)
End Function

Const BLOCK_SIZE:Int = 64

Type TPriorityHeap
	
	Field _data:THeapNode[]
	Field _heapPtr:Int

	Method New()
		_data = New THeapNode[BLOCK_SIZE]
	End Method
	
	Rem
		Adds a new element to the heap
	End Rem
	Method add(priority:Int, obj:Object)
		_reallocateHeap()
		
		' add the element at the end of the heap
		_data[_heapPtr] = THeapNode.Create(priority, obj)
		_heapPtr:+1
		
		_siftUp(_heapPtr-1)
	End Method

	Rem
		Adds a new element to the heap
	End Rem
	Method addNode(node:THeapNode)
		_reallocateHeap()
		
		' add the element at the end of the heap

		_data[_heapPtr] = node
		_heapPtr:+1
		
		_siftUp(_heapPtr-1)
	End Method
	
	Rem
		Returns true if the heap is empty
	End Rem
	Method isEmpty:Byte()
		Return (_heapPtr = 0)
	End Method
	
	Rem
		Returns the element with highest priority from the heap
	End Rem
	Method removeHighest:THeapNode()

		' no nodes left 
		If _heapPtr = 0 Return Null

		Local tmpNode:THeapNode = _data[0]
		' swap lowest with last heap item 
		_data[0] = _data[_heapPtr-1]
		_heapPtr:-1
		' restore heap condition 
		_heapify(0)
		Return tmpNode
	End Method
	
	Rem
		allocates new space block wise for extra speed
	End Rem
	Method _reallocateHeap()
		If _heapPtr => _data.Length-1
			_data = _data[.._data.length+BLOCK_SIZE]
		EndIf
	End Method
	
	Rem
		Restores the heap condition after inserting a new item
	End Rem
	Method _siftUp(i:Int)
		Local father:Int = Floor(i/2)
		While i > 0 And _data[i].priority() < _data[father].priority()
			Local tmpNode:THeapNode = _data[i]
			_data[i] = _data[father]
			_data[father] = tmpNode
			i = father
			father = Floor(i/2)
		Wend
	End Method
	
	Rem
		Heap items with lower priority go down the heap until the heap condition is met
	End Rem
	Method _heapify(i:Int)
		Local leftId:Int = 2*i
		Local rightId:Int = 2*i+1
		Local smallest:Int = i

		If (leftId < _heapPtr) And (_data[leftId].priority() < _data[i].priority())
			smallest = leftId
		End If
		
		If (rightId < _heapPtr) And (_data[rightId].priority() < _data[smallest].priority())
			smallest = rightId
		End If
		
		If i <> smallest
			Local tmpNode:THeapNode = _data[i]
			_data[i] = _data[smallest]
			_data[smallest] = tmpNode
			_heapify(smallest)
		End If
	End Method
	
	Rem
		DEBUG prints out the heap
	End Rem
	Method prnt()
		For Local i:Int = 0 Until _heapPtr
			Print _data[i].priority()
		Next
	End Method
	
	Rem
		Eine Contains - funktion.
	end rem
	Method Contains:Byte(Value:Object)
		For Local i:Int = 0 Until _heapPtr
			If _data[i]._data = Value Then Return 1
		Next
		Return 0
	End Method
End Type

Rem
	The heap node stores the priority and the associated object
End Rem
Type THeapNode
	
	Field _priority:Int
	Field _data:Object
	
	Rem
		Creates a new heap node and returns it
	End Rem
	Function Create:THeapNode(priority:Int, data:Object)
		Local tmpNode:THeapNode = New THeapNode
		tmpNode._priority = priority
		tmpNode._data = data
		Return tmpNode
	End Function
	
	Method priority:Int()
		Return _priority
	End Method
	
	Method data:Object()
		Return _data
	End Method
	
End Type

SuperStrict

Const MAP_W:Int = 512
Const MAP_H:Int = 512

Const ALLOW_DIAGONALS:Int = 1

Graphics MAP_W, MAP_H

Local Pixmap:TPixmap = CreatePixmap(MAP_W, MAP_H, PF_RGBA8888)

Local MapData:Byte[MAP_W, MAP_H]

Local AStar:TAStar = New TAStar.Create(MapData)
Local BFS:TBFS = New TBFS.Create(MapData)

Local StartX:Int = 20
Local StartY:Int = 20
Local EndX:Int = 500
Local EndY:Int = 500

Print "Linke Maustaste gedrueckt halten: Hindernisse setzen"
Print "Enter: Breitensuche starten"
Print "Leertaste: A* starten"
Print ""

Local Timer:TTimer = CreateTimer(60)
While Not (KeyHit(KEY_ESCAPE) Or AppTerminate())
	If MouseDown(1) Then
		DrawCircle(MouseX(), MouseY(), 10, MapData)
		RenderMap(Pixmap, MapData)
	EndIf
	
	If KeyHit(KEY_SPACE) Then
		Local Time:Int = MilliSecs()
		Local Node:TAStarNode = AStar.Search(StartX, StartY, EndX, EndY)
		
		Print "A*: " + (MilliSecs() - Time) + "ms"
		
		Node = AStar.GraphicalSearch(StartX, StartY, EndX, EndY)
		
		SetColor(255, 0, 0)
		While Node
			Plot Node.X, Node.Y
			Node = Node.Pred
		Wend
		
		AStar.Clear() 'Braucht einiges an Rechenzeit, aber wenn man den Speicher braucht, ist das ganz nützlich
		
		Flip 0
		WaitKey
	EndIf
	
	If KeyHit(KEY_ENTER) Then
		Local Time:Int = MilliSecs()
		Local Node:TBFSNode = BFS.Search(StartX, StartY, EndX, EndY)
		
		Print "Breitensuche: " + (MilliSecs() - Time) + "ms"
		
		Node = BFS.GraphicalSearch(StartX, StartY, EndX, EndY)
		
		SetColor(255, 0, 0)
		While Node
			Plot Node.X, Node.Y
			Node = Node.Pred
		Wend
		
		Flip 0
		WaitKey
	EndIf
	
	DrawPixmap Pixmap, 0, 0
	
	SetColor 255, 255, 255
	DrawRect StartX - 3, StartY - 3, 6, 6
	DrawRect EndX - 3, EndY - 3, 6, 6
	
	Flip 0
	WaitTimer Timer
Wend
End

Extern
	Function Memset(Dest:Byte Ptr, Value:Int, Count:Int) = "memset"
EndExtern

Function DrawCircle(X:Int, Y:Int, Radius:Int, MapData:Byte[,])
	For Local MX:Int = Max(X - Radius, 0) Until Min(X + Radius, MAP_W)
		For Local MY:Int = Max(Y - Radius, 0) Until Min(Y + Radius, MAP_H)
			If (MX - X)*(MX - X) + (MY - Y)*(MY - Y) < Radius*Radius Then MapData[MX, MY] = 1
		Next
	Next
End Function

Function RenderMap(Pixmap:TPixmap, MapData:Byte[,])
	For Local Y:Int = 0 Until MAP_H
		For Local X:Int = 0 Until MAP_W
			If MapData[X, Y] Then
				Pixmap.WritePixel(X, Y, $FFCCCCCC)
			Else
				Pixmap.WritePixel(X, Y, $00000000)
			EndIf
		Next
	Next
End Function

Type TAStar
	Const  UNVISITED:Int = %00
	Const CLOSED_SET:Int = %01
	Const   OPEN_SET:Int = %10
	Const SQR2:Float = 1.414213
	
	Field Queue:TFibonacciHeap
	
	Field MapW:Int
	Field MapH:Int
	
	Field MapData:Byte[,]
	Field WayNodes:TAStarNode[,]
	Field NodeFlags:Byte[,]
	
	Method New()
		Queue = New TFibonacciHeap
	End Method
	
	Method Clear()
		Queue.Clear()
		
		For Local X:Int = 0 Until MapW
			For Local Y:Int = 0 Until MapH
				If WayNodes[X, Y] Then
					WayNodes[X, Y].TreeNode = Null
					WayNodes[X, Y].Pred = Null
					WayNodes[X, Y] = Null
				EndIf
			Next
		Next
	End Method
	
	Method Search:TAStarNode(StartX:Int, StartY:Int, EndX:Int, EndY:Int)
		Memset(Varptr NodeFlags[0, 0], 0, MapW*MapH)
		Queue.Clear()
		
		Local DirX:Int[] =   [  -1,  0,   1,   0,   -1,   -1,    1,    1]
		Local DirY:Int[] =   [  0,   1,   0,  -1,   -1,    1,   -1,    1]
		Local Cost:Float[] = [1.0, 1.0, 1.0, 1.0, SQR2, SQR2, SQR2, SQR2]
		
		If Not WayNodes[StartX, StartY] Then WayNodes[StartX, StartY] = New TAStarNode
		WayNodes[StartX, StartY].Create(StartX, StartY, EndX, EndY, 0.0, Null, Queue)
		
		While Queue.FindMinimum()
			Local Node:TAStarNode = TAStarNode(Queue.ExtractMinimum().Value)
			
			If Node.X = EndX And Node.Y = EndY Then Return Node
			
			NodeFlags[Node.X, Node.Y] = CLOSED_SET
			WayNodes[Node.X, Node.Y].TreeNode = Null
			
			For Local I:Int = 0 Until 4 + 4*ALLOW_DIAGONALS
				Local X:Int = Node.X + DirX[I]
				Local Y:Int = Node.Y + DirY[I]
				
				If X >= 0 And Y >= 0 And X < MapW And Y < MapH And Not MapData[X, Y] Then
					If NodeFlags[X, Y] = UNVISITED Then
						NodeFlags[X, Y] = OPEN_SET
						
						If Not WayNodes[X, Y] Then WayNodes[X, Y] = New TAStarNode
						WayNodes[X, Y].Create(X, Y, EndX, EndY, Cost[I], Node, Queue)
					ElseIf NodeFlags[X, Y] = OPEN_SET Then
						If Node.G + Cost[	I] < WayNodes[X, Y].G Then WayNodes[X, Y].Improve(Cost[I], Node, Queue)
					EndIf
				EndIf
			Next
		Wend
		
		Return Null
	End Method
	
	Method GraphicalSearch:TAStarNode(StartX:Int, StartY:Int, EndX:Int, EndY:Int)
		Memset(Varptr NodeFlags[0, 0], 0, MapW*MapH)
		Queue.Clear()
		
		Local DirX:Int[] =   [  -1,  0,   1,   0,   -1,   -1,    1,    1]
		Local DirY:Int[] =   [  0,   1,   0,  -1,   -1,    1,   -1,    1]
		Local Cost:Float[] = [1.0, 1.0, 1.0, 1.0, SQR2, SQR2, SQR2, SQR2]
		
		If Not WayNodes[StartX, StartY] Then WayNodes[StartX, StartY] = New TAStarNode
		WayNodes[StartX, StartY].Create(StartX, StartY, EndX, EndY, 0.0, Null, Queue)
		
		Local Iteration:Int
		While Queue.FindMinimum()
			Local Node:TAStarNode = TAStarNode(Queue.ExtractMinimum().Value)
			
			If Node.X = EndX And Node.Y = EndY Then Return Node
			
			NodeFlags[Node.X, Node.Y] = CLOSED_SET
			WayNodes[Node.X, Node.Y].TreeNode = Null
			
			SetColor 64, 64, 64
			Plot Node.X, Node.Y
			
			For Local I:Int = 0 Until 4 + 4*ALLOW_DIAGONALS
				Local X:Int = Node.X + DirX[I]
				Local Y:Int = Node.Y + DirY[I]
				
				If X >= 0 And Y >= 0 And X < MapW And Y < MapH And Not MapData[X, Y] Then
					If NodeFlags[X, Y] = UNVISITED Then
						NodeFlags[X, Y] = OPEN_SET
						
						SetColor 0, 255, 0
						Plot X, Y
						
						If Not WayNodes[X, Y] Then WayNodes[X, Y] = New TAStarNode
						WayNodes[X, Y].Create(X, Y, EndX, EndY, Cost[I], Node, Queue)
					ElseIf NodeFlags[X, Y] = OPEN_SET Then
						If Node.G + Cost[	I] < WayNodes[X, Y].G Then WayNodes[X, Y].Improve(Cost[I], Node, Queue)
					EndIf
				EndIf
			Next
			
			Iteration :+ 1
			
			If (Iteration Mod 50) = 0 Then Flip 0; PollSystem()
		Wend
		
		Return Null
	End Method
	
	Method Create:TAstar(MapData:Byte[,])
		Self.MapData = MapData
		
		Local Dim:Int[] = MapData.Dimensions()
		MapW = Dim[0]
		MapH = Dim[1]
		
		WayNodes  = New TAStarNode[MapW, MapH]
		NodeFlags = New Byte[MapW, MapH]
		
		Return Self
	End Method
End Type

Type TAStarNode
	Global Count:Int
	
	Field X:Int
	Field Y:Int
	
	Field G:Float
	Field H:Float
	
	Field Pred:TAStarNode
	Field TreeNode:TTreeNode
	
	Method New()
		Count :+ 1
	End Method
	
	Method Delete()
		Count :- 1
	End Method
	
	Method Improve(Cost:Float, Pred:TAStarNode, Queue:TFibonacciHeap)
		Self.Pred = Pred
		
		Queue.DecreaseKey(TreeNode, Pred.G + Cost + H)
		
		G = Pred.G + Cost
	End Method
	
	Method Create:TAstarNode(X:Int, Y:Int, EndX:Int, EndY:Int, Cost:Float, Pred:TAStarNode, Queue:TFibonacciHeap)
		Self.X = X
		Self.Y = Y
		
		H = Sqr((X - EndX)*(X - EndX) + (Y - EndY)*(Y - EndY))
		
		Self.Pred = Pred
		If Pred Then
			G = Pred.G + Cost
		Else
			G = Cost
		EndIf
		
		TreeNode = Queue.Insert(G + H, Self)
		
		Return Self
	End Method
End Type

Type TBFS
	Field WorkList:TList
	
	Field MapW:Int
	Field MapH:Int
	
	Field MapData:Byte[,]
	Field Visited:Byte[,]
	
	Method Search:TBFSNode(StartX:Int, StartY:Int, EndX:Int, EndY:Int)
		Memset(Varptr Visited[0, 0], 0, MapW*MapH)
		WorkList.Clear()
		
		Visited[StartX, StartY] = True
		WorkList.AddLast(New TBFSNode.Create(StartX, StartY, Null))
		
		Local DirX:Int[] =   [  -1,  0,   1,   0,   -1,   -1,    1,    1]
		Local DirY:Int[] =   [  0,   1,   0,  -1,   -1,    1,   -1,    1]
		
		While Not WorkList.IsEmpty()
			Local Node:TBFSNode = TBFSNode(WorkList.RemoveFirst())
			
			If Node.X = EndX And Node.Y = EndY Then Return Node
			
			For Local I:Int = 0 Until 4 + 4*ALLOW_DIAGONALS
				Local X:Int = Node.X + DirX[I]
				Local Y:Int = Node.Y + DirY[I]
				
				If X >= 0 And Y >= 0 And X < MapW And Y < MapH And Not MapData[X, Y] And Not Visited[X, Y] Then
					WorkList.AddLast(New TBFSNode.Create(X, Y, Node))
					Visited[X, Y] = True
				EndIf
			Next
		Wend
		
		Return Null
	End Method
	
	Method GraphicalSearch:TBFSNode(StartX:Int, StartY:Int, EndX:Int, EndY:Int)
		Memset(Varptr Visited[0, 0], 0, MapW*MapH)
		WorkList.Clear()
		
		Visited[StartX, StartY] = True
		WorkList.AddLast(New TBFSNode.Create(StartX, StartY, Null))
		
		Local DirX:Int[] =   [  -1,  0,   1,   0,   -1,   -1,    1,    1]
		Local DirY:Int[] =   [  0,   1,   0,  -1,   -1,    1,   -1,    1]
		
		Local Iteration:Int
		While Not WorkList.IsEmpty()
			Local Node:TBFSNode = TBFSNode(WorkList.RemoveFirst())
			
			If Node.X = EndX And Node.Y = EndY Then Return Node
			
			SetColor(64, 64, 64)
			Plot Node.X, Node.Y
			
			For Local I:Int = 0 Until 4 + 4*ALLOW_DIAGONALS
				Local X:Int = Node.X + DirX[I]
				Local Y:Int = Node.Y + DirY[I]
				
				If X >= 0 And Y >= 0 And X < MapW And Y < MapH And Not MapData[X, Y] And Not Visited[X, Y] Then
					WorkList.AddLast(New TBFSNode.Create(X, Y, Node))
					Visited[X, Y] = True
					
					SetColor(0, 255, 0)
					Plot X, Y
				EndIf
			Next
			
			Iteration :+ 1
			
			If (Iteration Mod 50) = 0 Then Flip 0; PollSystem()
		Wend
		
		Return Null
	End Method
	
	Method Create:TBFS(MapData:Byte[,])
		WorkList = New TList
		
		Self.MapData = MapData
		
		Local Dimensions:Int[] = MapData.Dimensions()
		MapW = Dimensions[0]
		MapH = Dimensions[1]
		
		Visited = New Byte[MapW, MapH]
		
		Return Self
	End Method
End Type

Type TBFSNode
	Field X:Int
	Field Y:Int
	
	Field Pred:TBFSNode
	
	Method Create:TBFSNode(X:Int, Y:Int, Pred:TBFSNode)
		Self.X = X
		Self.Y = Y
		Self.Pred = Pred
		
		Return Self
	End Method
End Type

Type TBoxedFloat
	Field Value:Float
	
	Method Create:TBoxedFloat(Value:Float)
		Self.Value = Value
		
		Return Self
	End Method
End Type

Type TFibonacciHeap
	Const MINUS_INFINITY:Float = -1.0/0.0
	
	Field Minimum:TTreeNode
	
	Field ChildCount:Int
	
	Field Trees:TLinkedList
	
	Method New()
		Trees = New TLinkedList
	End Method
	
	Method Clear()
		Local Root:TListLink = Trees.FirstLink()
		While Root
			Local Succ:TListLink = Root.Succ
			TTreeNode(Root.Value).Clear()
			
			Root.Pred = Null
			Root.Value = Null
			Root.Succ = Null
			
			Root = Succ
		Wend
		
		Minimum = Null
		ChildCount = 0
		Trees.Clear()
	End Method
	
	Method Merge(Other:TFibonacciHeap)
		Trees = Trees.Merge(Other.Trees)
		ChildCount :+ Other.ChildCount
		
		If Not Minimum Or (Other.Minimum <> Null And Other.Minimum.Priority < Minimum.Priority) Then Minimum = Other.Minimum
	End Method
	
	Method Insert:TTreeNode(Priority:Float, Value:Object)
		Local Node:TTreeNode = New TTreeNode.Create(Priority, Value)
		
		Node.Link = Trees.AddLast(Node)
		
		ChildCount :+ 1
		
		If Minimum = Null Or Priority < Minimum.Priority Then Minimum = Node
		
		Return Node
	End Method
	
	Method FindMinimum:TTreeNode()
		Return Minimum
	End Method
	
	Method ExtractMinimum:TTreeNode()
		If Minimum Then
			Local OldMinimum:TTreeNode = Minimum
			Minimum = Null
			
			Trees.Remove(OldMinimum.Link)
			
			If Trees.IsEmpty() Then
				Trees = OldMinimum.Childs
			Else
				Trees = Trees.Merge(OldMinimum.Childs)
				OldMinimum.Childs.Head = Null
				OldMinimum.Childs.Tail = Null
				OldMinimum.Childs = Null
			EndIf
			
			Local TreeDegrees:TTreeNode[FindHighestBit(ChildCount) + 1] 'log2(ChildCount)
			
			Local NextLink:TListLink = Trees.FirstLink(), Root:TTreeNode
			While NextLink
				If Not Root Then Root = TTreeNode(NextLink.Value)
				Root.Marked = False
				Root.Parent = Null
				
				Local DegreeCell:TTreeNode = TreeDegrees[Root.Degree]
				If DegreeCell Then
					TreeDegrees[Root.Degree] = Null
					If DegreeCell.Priority < Root.Priority Then
						DegreeCell.AddChild(Root)
						Root = DegreeCell
					Else
						Root.AddChild(DegreeCell)
					EndIf
				Else
					TreeDegrees[Root.Degree] = Root
					NextLink = NextLink.NextLink()
					Root = Null
				EndIf
			Wend
			
			Local Link:TListLink = Trees.FirstLink()
			While Link
				Local Succ:TListLink = Link.Succ
				
				Link.Pred = Null
				Link.Value = Null
				Link.Succ = Null
				
				Link = Succ
			Wend
			Trees.Clear()
			
			For Local Root:TTreeNode = EachIn TreeDegrees
				If Root Then
					If Minimum = Null Or Minimum.Priority > Root.Priority Then Minimum = Root
					Root.Link = Trees.AddLast(Root)
				EndIf
			Next
			
			ChildCount :- 1
			
			OldMinimum.Link = Null
			
			Return OldMinimum
		EndIf
	End Method
	
	Method DecreaseKey(Node:TTreeNode, NewPriority:Float)
		?Debug
			Assert NewPriority <= Node.Priority, "New priority must not be greater!"
		?
		
		Node.Priority = NewPriority
		
		If Node.Parent And Node.Priority < Node.Parent.Priority Then
			While True
				Node.Parent.Childs.Remove(Node.Link)
				Node.Parent.Degree :- 1
				Node.Link = Trees.AddLast(Node)
				Node.Marked = False
				
				If Node.Parent.Parent Then
					If Node.Parent.Marked Then
						Local Parent:TTreeNode = Node.Parent
						Node.Parent = Null
						Node = Parent
					Else
						Node.Parent.Marked = True
						Node.Parent = Null
						Exit
					EndIf
				Else
					Node.Parent = Null
					Exit
				EndIf
			Wend
			
			If Node.Priority < Minimum.Priority Then Minimum = Node
		EndIf
	End Method
	
	Method DeleteKey(Node:TTreeNode)
		DecreaseKey(Node, MINUS_INFINITY)
		ExtractMinimum()
	End Method
	
	Function FindHighestBit:Int(Value:Int)
		Local Index:Int
		While Value
			Value :Shr 1
			Index :+ 1
		Wend
		
		Return Index
	End Function
End Type

Type TTreeNode
	Global Count:Int
	
	Field Priority:Float
	
	Field Parent:TTreeNode
	Field Link:TListLink
	
	Field Value:Object
	
	Field Marked:Byte
	
	Field Degree:Int
	Field Childs:TLinkedList
	
	Method New()
		Count :+ 1
	End Method
	
	Method Delete()
		Count :- 1
	End Method
	
	Method Create:TTreeNode(Priority:Float, Value:Object)
		Self.Priority = Priority
		Self.Value = Value
		Self.Childs = New TLinkedList
		
		Return Self
	End Method
	
	Method AddChild(Node:TTreeNode)
		Degree :+ 1
		
		Node.Link   = Childs.AddLast(Node)
		Node.Parent = Self
	End Method
	
	Method Clear()
		Parent = Null
		Link = Null
		Value = Null
		
		Local Child:TListLink = Childs.FirstLink()
		While Child
			Local Succ:TListLink = Child.Succ
			TTreeNode(Child.Value).Clear()
			
			Child.Pred = Null
			Child.Value = Null
			Child.Succ = Null
			
			Child = Succ
		Wend
		Childs.Head = Null
		Childs.Tail = Null
		Childs = Null
	End Method
End Type

Type TLinkedList
	Global Count:Int
	
	Field Head:TListLink
	Field Tail:TListLink
	
	Field Length:Int
	
	Method New()
		Count :+ 1
	End Method
	
	Method Delete()
		Count :- 1
	End Method
	
	Method AddFirst:TListLink(Value:Object)
		Local Link:TListLink = New TListLink.Create(Value, Null, Head)
		
		If Not Tail Then Tail  = Link
		If Head Then Head.Pred = Link
		Head = Link
		
		Length :+ 1
		
		Return Link
	End Method
	
	Method AddLast:TListLink(Value:Object)
		Local Link:TListLink = New TListLink.Create(Value, Tail, Null)
		
		If Not Head Then Head  = Link
		If Tail Then Tail.Succ = Link
		Tail = Link
		
		Length :+ 1
		
		Return Link
	End Method
	
	Method LastLink:TListLink()
		Return Tail
	End Method
	
	Method FirstLink:TListLink()
		Return Head
	End Method
	
	Method IsEmpty:Int()
		Return Head = Null
	End Method
	
	Method Remove(Link:TListLink)
		If Head = Link Then Head = Link.Succ
		If Tail = Link Then Tail = Link.Pred
		
		Length :- 1
		
		Link.Remove()
	End Method
	
	Method Clear()
		Tail = Null
		Head = Null
		Length = 0
	End Method
	
	Method Merge:TLinkedList(Other:TLinkedList)
		If IsEmpty() Then Return Other
		
		If Not Other.IsEmpty() Then
			Length :+ Other.Length
			
			Tail.Succ = Other.Head
			Other.Head.Pred = Tail
			Tail = Other.Tail
		EndIf
		
		Return Self
	End Method
End Type

Type TListLink
	Global Count:Int
	
	Field Value:Object
	
	Field Succ:TListLink
	Field Pred:TListLink
	
	Method New()
		Count :+ 1
	End Method
	
	Method Delete()
		Count :- 1
	End Method
	
	Method Remove()
		If Succ Then Succ.Pred = Pred
		If Pred Then Pred.Succ = Succ
	End Method
	
	Method PreviousLink:TListLink()
		Return Pred
	End Method
	
	Method NextLink:TListLink()
		Return Succ
	End Method
	
	Method Create:TListLink(Value:Object, Pred:TListLink, Succ:TListLink)
		Self.Value = Value
		Self.Pred = Pred
		Self.Succ = Succ
		
		Return Self
	End Method
End Type