Type THeap
     Field _first:TNode
     Field _last:Tnode

     Field _nodeCnt:Int
     Field _nodeAVGprio:Float
End Type

Type TNode
     Field _data:Object
     Field _priority:Object

     Field _prevLink:TNode
     Field _nextLink:TNode
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
	
	Function Create:TAStern2D(startX:Int, startY:Int, zielX:Int, zielY:Int, mapArray:Short[][], attach_to_list:Byte = False)
		Local as:TAStern2D = New TAStern2D
		
		If mapArray[zielX][zielY] > 0 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
		
		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()
		
		Return as
		
	End Function
	
	Method Update:Int[][] (times:Int)
		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
			'holt den Besten knoten.
			
			If Self._offen.isEmpty() Then
				TAStern2D.list.Remove(Self)
				Self.impossible = 1
				Return Null
			End If
			knoten = TKnoten(Self._offen.removeHighest()._data)
			
			_geschlossen[knoten.posx, knoten.posy] = 1 'in GESCHLOSSEN eintragen. 
			__offenArr[knoten.posx, knoten.posy] = Null 'aus OFFEN entfernen.
			
			'position setzen
			x = knoten.posx
			y = knoten.posy
			
			If x = zielX And y = zielY Then
				Return Self.BuildWaypoint(knoten)
			End If
			
			'umliegende punkte überprüfen.
			For step_x = x - 1 To x + 1 Step 1
			For step_y = y - 1 To y + 1 Step 1
				'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(x - step_x)
					Local dirY:Int = Sgn(y - step_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
				
				'Check: Ist in offen?
				If __offenArr[step_x, step_y] Then
					'knoten ist offen. G vergleich.
					If __offenArr[step_x, step_y].g_costs > knoten.g_costs + g Then
						'umschreiben.
						__offenArr[step_x, step_y].ReLink(knoten, g)
						Continue
					End If
				End If
				
				'nicht geschlossen, nicht offen, nicht out of bounds: einfügen.
				newNode = TKnoten.Create(step_x, step_y, zielX, zielY, knoten, g + knoten.g_costs)
				__offenArr[step_x, step_y] = newNode
				_offen.add(newNode.f_costs, newNode)
			'	Print "node eingefügt."
			Next
			Next
		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 + knoten.g_costs
		Self.f_costs = Self.h_costs + Self.g_costs
	End Method
End Type

Const BLOCK_SIZE:Int = 16

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

Include "AStern Type.bmx"
Include "TPriorityHeap.bmx"

Graphics 800, 600

Const rasterSize:Int = 100

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

Global aStern_dauer:Int = 0

While Not KeyHit(KEY_ESCAPE) And Not AppTerminate()
	Cls
	
	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 = MouseX() - oriX
	Local my:Int = MouseY() - oriY
	
	If MouseHit(1) Then
		unit.AStern = TAStern2D.Create(unit.posx, unit.posy, (mx) / 10, (my) / 10, land.map, 1)
	EndIf
	If MouseDown(2) Then
		land.setBlock(mx / 10, my / 10, 1)
	End If
	
	
	TUnit.Update()
	
	land.draw()
	SetColor(255, 255, 255)
	DrawText("Dauer: " + aStern_dauer, -oriX + 50, -oriY + 50)
	
	Flip 1
	
	WaitTimer(timer)
	
Wend


Type TLandscape
	Field map:Short[][]
	
	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
	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 TUnit
	Global list:TList = CreateList()
	
	Field posx:Int
	Field posy:Int
	
	Field _X:Float
	Field _Y:Float
	
	Field aStern:TAStern2d
	
	Field waypoints: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 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 waypoints:Int[][] = AStern.Update(100)
			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
				Self.waypoints = waypoints
				Self.AStern = Null
			End If
		EndIf
		
		'wegpunkte abgehen.
		Local i:Int
		Local wp:Int[]
		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
		End If
	End Method
	
	Method _draw()
		DrawOval(posx * 10 - 5 + _X, posy * 10 - 5 + _Y, 10, 10)
	End Method
End Type

Global Feld%[75,75] 'ist das eigentliche Spielfeld

Global Checked%[75,75] ' ist für die Messergebnisse

Type TStelle
	Field x%,y%
End Type

' Trick I: es wird so getan als wären es zu jeden Punkt des Spielfeldes anstrengende 999 Schritte:
For Local i%=0 To 74
	For Local j%=0 To 74
		checked[i,j]=999
	Next
Next




' erstes Objekt in der Liste ist die ZielPos:
	Local Stelle:TStelle=New TStelle
	Stelle.X=ZielX
	Stelle.Y=ZielY
	ListAddLast Liste , Stelle

Repeat

		Local i%=0
		For Local loc:TStelle = EachIn Liste
			i=i+1
			' das ist der Trick II, dank i% werden immer erst alle alten TList-Einträge abgearbeitet:
			' (Anmerkung: geht auch ohne dies, solange die Kosten überallhin gleich sind)
			If i>Anzahl Then Exit
			Scanne loc
		Next
		'Die Liste wird mittendrin abgebrochen, die Elemente gezählt und dann wieder von vorne begonnen
		' bevor es von vorne losgeht, könnte man hier sortieren
		Anzahl=CountList(Liste)
		
		' Abbruch, wenn alle Felder gescannt sind:
		If Anzahl=0 Then Exit 
		
' oder Abbruch, wenn das Ziel entdeckt wurde:		
Until Checked[SuchX,SuchY]<999

ZeigeWeg StartX, StartY

End




Function Scanne()
	Local Preis%
      X=loc.X
	Y=loc.Y
	Preis=Checked[x,y] 
	' zunächst wird die aktuelle Stelle aus der Liste gelöscht 
	ListRemove liste, loc
	
	' dann werden ihre 4 Nachbar möglicherweise aufgenommen:
		Aufnehmen (X+1,y,Preis+1)
		Aufnehmen (X-1,y,Preis+1)
		Aufnehmen (X,y+1,Preis+1)
		Aufnehmen (X,y-1,Preis+1)
End Function




Function Aufnehmen(X%,Y%,Preis%)
	' nur ein Test gegen die Spielfeldränder
	If x<0 Or x>74 Or y<0 Or y>74 
		Return
	' ein Test auf Mauer an dieser Stelle	
	ElseIf Feld[x,y]=1 
		' Checked=0 bedeutet unerreichbar
		Checked[x,y]=0
		Return
	EndIf

	' jetzt der Check auf bereits früher gecheckt:
	If Checked[x,y]>Preis 
		' es war noch nie so leicht hierher zu kommen, also Aufnahme:
		Checked[x,y]=Preis
		Local Stelle:TStelle=New TStelle
		Stelle.X=X
		Stelle.Y=Y
		ListAddLast Liste , Stelle
	EndIf
End Function




Function ZeigeWeg(X%,y%)
	Repeat 
		' was kostet es hier?
		Wert=Checked[x,y]
		' und wohin (4 Himmelsrichtungen)wäre es billiger?
		If Checked[x-1,y]= wert-1
			x=x-1
		ElseIf Checked[x+1,y]= wert-1
			x=x+1
		ElseIf Checked[x,y-1]= wert-1
			y=y-1
		ElseIf Checked[x,y+1]= wert-1
			y=y+1
		EndIf
		
		Print "gehe nach " + x + " " + y
	'dort, wo die kosten 0 sind, haben wir Ziel erreicht: 	
	Until Wert=0
End Function