AppTitle "Pathfinding gesamt"
Graphics 640,480,16,2
SetBuffer BackBuffer()

Const max = 1500
Const le = 102

Dim map(15,11)

Dim path(max,le,4)

Global num,mx,my,startx,starty,zielx,ziely,ziel,mog

Function draw()
For y = 0 To 11
For x = 0 To 15

Select map(x,y)
Case 0
Color 180,0,0
Case 1
Color 0,180,0
End Select
Rect x*40,y*40,40,40

Color 0,0,0
Rect x*40,y*40,40,40,0
Next
Next

Color 0,0,180
Oval startx+10,starty+10,20,20

Color 180,180,0
Oval zielx+10,ziely+10,20,20

;Pfad zum Ziel zeichnen
Color 250,0,200
If ziel = -1 Then
Text 0,0,"Nicht möglich"
Else
For j = 0 To path(ziel,0,4)-1
Rect path(ziel,j,1)*40+19,path(ziel,j,2)*40+19,3,3
Next
EndIf

End Function

Function fill()
Repeat
mx = MouseX()
my = MouseY()

If MouseDown(1) Then map(mx/40,my/40)=1
If MouseDown(2) Then map(mx/40,my/40)=0
If KeyDown(28) Then
startx = mx/40*40
starty = my/40*40
EndIf
If KeyDown(157) Then
zielx = mx/40*40
ziely = my/40*40
EndIf
draw()

Flip
Cls
Until KeyHit(1)
FlushKeys()
End Function

Function doppelpunkt(x,y,p)
zwei = 0
For l = 0 To path(p,0,4)-1
If (path(p,l,1)=x) And (path(p,l,2)=y) Then
zwei = 1
Exit
EndIf
Next

Return zwei
End Function

Function create_path(x,y)
path(num,0,3)=1
path(num,0,1)=x
path(num,0,2)=y
path(num,0,4)=1

If num < max Then num = num + 1
End Function

Function addpoint(x,y,p)
If path(p,0,3)=1 Then

path(p,path(p,0,4),3)=1
path(p,path(p,0,4),1)=x
path(p,path(p,0,4),2)=y
path(p,0,4)=path(p,0,4)+1
 If path(p,0,4)>le Then
 path(p,0,4)=le
 discard_path(p)
 EndIf

EndIf
End Function

Function copy_path(n)
For b = 0 To le
For c = 1 To 4
path(num,b,c)=path(n,b,c)
Next
Next

If num < max Then num = num + 1
End Function

Function kill_path(pat)
For z = pat To max-1
For b = 0 To le
For c = 1 To 4
path(z,b,c)=path(z+1,b,c)
Next
Next
Next
num = num - 1
End Function

Function discard_path(pat)
path(pat,0,3)=2
End Function

Function pfadfinden(pa)
dirs = 0
o = 0
u = 0
l = 0
r = 0
pt = path(pa,0,4)-1
px=path(pa,pt,1)
py=path(pa,pt,2)

;Bei Ziel angekommen kann man aufhören
If (path(pa,pt,1)=zielx/40) And (path(pa,pt,2)=ziely/40) Then
discard_path(pa)
EndIf

If path(pa,0,3)=1 Then

;Wo ist ein freier Weg?
If px>0 Then
 If map(px-1,py)<>0 Then
 dirs = dirs + 1
 l = 1
 EndIf
EndIf
If px<15 Then
 If map(px+1,py)<>0 Then
 dirs = dirs + 1
 r = 1
 EndIf
EndIf
If py>0 Then
 If map(px,py-1)<>0 Then
 dirs = dirs + 1
 o = 1
 EndIf
EndIf
If py<11 Then
 If map(px,py+1)<>0 Then
 dirs = dirs + 1
 u = 1
 EndIf
EndIf

;War ich da schon?
If l = 1 Then
 If doppelpunkt(px-1,py,pa)=1 Then
 l = 0
 dirs = dirs - 1
 EndIf
EndIf
If r = 1 Then
 If doppelpunkt(px+1,py,pa)=1 Then
 r = 0
 dirs = dirs - 1
 EndIf
EndIf
If o = 1 Then
 If doppelpunkt(px,py-1,pa)=1 Then
 o = 0
 dirs = dirs - 1
 EndIf
EndIf
If u = 1 Then
 If doppelpunkt(px,py+1,pa)=1 Then
 u = 0
 dirs = dirs - 1
 EndIf
EndIf

;Kein Ausweg
If dirs = 0 Then
DebugLog "Kein Ausweg"
discard_path(pa)
EndIf

;Ein Ausweg
If dirs = 1 Then
DebugLog "Ein Ausweg"
If l = 1 Then addpoint(px-1,py,pa)
If r = 1 Then addpoint(px+1,py,pa)
If o = 1 Then addpoint(px,py-1,pa)
If u = 1 Then addpoint(px,py+1,pa)
EndIf

;Zwei Auswege
If dirs = 2 Then
 DebugLog "Zwei Auswege"
 
 ;Einen neuen Pfad eröffnen
     If l = 1 Then
  copy_path(pa)
  addpoint(px-1,py,num-1)
  l = 0
 ElseIf r = 1 Then
  copy_path(pa)
  addpoint(px+1,py,num-1)
  r = 0
 ElseIf o = 1 Then
  copy_path(pa)
  addpoint(px,py-1,num-1)
  o = 0
 ElseIf u = 1 Then
  copy_path(pa)
  addpoint(px,py+1,num-1)
  u = 0
  EndIf

 ;In die andere verbleibende Richtung weitergehen
 If l = 1 Then addpoint(px-1,py,pa)
 If r = 1 Then addpoint(px+1,py,pa)
 If o = 1 Then addpoint(px,py-1,pa)
 If u = 1 Then addpoint(px,py+1,pa)
EndIf

;Drei Auswege
If dirs = 3 Then
 DebugLog "Drei Auswege"
 
 ;Zwei neue Pfade eröffnen
 For m = 1 To 2
     If l = 1 Then
  copy_path(pa)
  addpoint(px-1,py,num-1)
  l = 0
 ElseIf r = 1 Then
  copy_path(pa)
  addpoint(px+1,py,num-1)
  r = 0
 ElseIf o = 1 Then
  copy_path(pa)
  addpoint(px,py-1,num-1)
  o = 0
 ElseIf u = 1 Then
  copy_path(pa)
  addpoint(px,py+1,num-1)
  u = 0
  EndIf
 Next

 ;In die andere verbleibende Richtung weitergehen
 If l = 1 Then addpoint(px-1,py,pa)
 If r = 1 Then addpoint(px+1,py,pa)
 If o = 1 Then addpoint(px,py-1,pa)
 If u = 1 Then addpoint(px,py+1,pa)
EndIf

;Vier Auswege
If dirs = 4 Then
 DebugLog "Vier Auswege"
 
 ;Drei neue Pfade eröffnen nach oben, rechts und unten
  copy_path(pa)
  addpoint(px+1,py,num-1)
  r = 0
  copy_path(pa)
  addpoint(px,py-1,num-1)
  o = 0
  copy_path(pa)
  addpoint(px,py+1,num-1)
  u = 0

 ;selbst nach links weitergehen
 addpoint(px-1,py,pa)
EndIf

EndIf

End Function

Function invade()
num = 0
ziel = -1

create_path(startx/40,starty/40)

;Wiederholen alle Pfade beendet wurden
Repeat
active = 0
For pf = 0 To max
;Wenn der erste Punkt existiert existiert aus der Pfad
If path(pf,0,3)=1 Then
DebugLog "Pfad "+Str$(pf)+":"
pfadfinden(pf)
active = active + 1
EndIf
Next

DebugLog ""
Until active = 0

mog = 0
short = 0
leng = 999
For i = 0 To max
If (path(i,0,3)=2) And (path(i,0,4)>0) Then
pl = path(i,0,4)

If (path(i,pl-1,1)=zielx/40) And (path(i,pl-1,2)=ziely/40) Then
 If pl<leng Then
 leng = pl
 short = i
 mog = 1
 EndIf
EndIf
EndIf
Next

If mog = 1 Then ziel = short
DebugLog "Ziel : "+Str$(ziel)

End Function

fill()

invade()
Repeat

draw()

Flip
Cls
Until KeyHit(1)
End

Type node
   Field x
   Field y
   Field parent
   Field cost
End Type

Type todo
   Field id
End type

Global sx = 50
Global sy = 50
Global fs = 20
Global stx = 20
Global sty = 25
Global zix = 30
Global ziy = 25
ftimer = CreateTimer(10)
Dim feld(sx,sy)

For y = 20 To 30
   feld(25,y) = 1
Next

Graphics (sx+1)*fs, (sy+1)*fs,16, 2
ClsColor 100,100,100
Repeat
notfound = 0
If stx = zix And sty = ziy Then ende("Ziel Erreicht")
createnode(stx,sty,0,0)
todo.todo = First todo
node.node = First node
Repeat
   If KeyHit(57) Then s = 1
   If KeyHit(1) Then End
   If s
      If best() = 0 Then notfound = 1 Exit
      todo.todo = object.todo(best())
      node.node = Object.node(todo\id)
      
      For difx = -1 To 1
         For dify = -1 To 1
            If Not nodeexist(node\x+difx,node\y+dify)
               If Not feld(node\x+difx,node\y+dify) createnode(node\x+difx,node\y+dify,todo\id,node\cost+abs(difx)+abs(dify))
            end if
         Next
      Next
      
      Delete todo.todo
   Else
      Gosub drawscreen
      Flip
   End If
   Until nodeexist(zix,ziy)
   
   Gosub drawscreen
   id = nodeexist(zix,ziy)
   node.node = Object.node(id)
   Color 255,0,0
   If notfound Then ende("Es gibt keinen Weg")
   If Not notfound
      Repeat
         node2.node = Object.node(node\parent)
         If Handle(node2) Line node\x*fs + fs/2,node\y*fs + fs/2, node2\x*fs + fs/2,node2\y*fs + fs/2
         oldnode.node = node.node
         node.node = node2.node
      Until node\x = stx And node\y = sty
   End if
   WaitTimer ftimer
   Flip
   stx = oldnode\x
   sty = oldnode\y
   Delete Each node
   Delete Each todo
Forever

end
.drawscreen
   cls
   For x = 0 To sx
      For y = 0 To sy
         If feld(x,y) Color 0,0,0 Else Color 200,200,200
         If stx = x And sty = y Color 255,0,0
         If zix = x And ziy = y Color 0,0,255
         Rect x*fs,y*fs,fs-1,fs-1
         If rectsoverlap(mousex(), MouseY(), 1, 1, x*fs,y*fs,fs,fs)
          Color 0,128,0: Rect x*fs,y*fs,fs-1,fs-1, 0
          If Mousedown(1) feld(x,y) = 1
          If MouseDown(2) feld(x,y) = 0
         End if
      Next
   Next
  Color 0,0,0
;
   For node.node = Each node
      gescost = node\cost + Abs(node\x-zix) + Abs(node\y-ziy)
    node2.node = Object.node(node\parent)
    Color 128,128,128
    If Handle(node2) Line node\x*fs + fs/2,node\y*fs + fs/2, node2\x*fs + fs/2,node2\y*fs + fs/2
    Color 0,0,0
    Text node\x*fs + fs/2, node\y*fs + fs/2, node\cost,1,1

   next
   
return

Function createnode(x,y,p,c,atdt=1)
   node.node = New node
   node\x = x
   node\y = y
   node\parent = p
   node\cost = c
   If atdt
      todo.todo = New todo
      todo\id = Handle(node)
   End If
   Return Handle(node)
End Function

Function best()
   bestcost = 10^5
   For todo.todo = Each todo
      node.node = Object.node(todo\id)
      gescost = node\cost + Abs(node\x-zix) + Abs(node\y-ziy)
   If gescost < bestcost Then bestcost = gescost: ret = Handle(todo)
   Next
   Return ret
End Function

Function nodeexist(x,y)
   If x < 0 Return -1
   If y < 0 Return -1
   If x > sx Return -1
   If y > sy Return -1
   For node.node = Each node
      If node\x = x And node\y = y Return Handle(node)
   next
End Function

Function ende(t$)
   Notify t$ ;Falls kein b+. diese Zeile auskommentieren oder mit Print ersetzen
   End
End function

;PATHFINDER

Global MapWidth%
Global MapHeight%

Dim Map%(0 , 0)
Dim PathDir%(0 , 0)
Dim tmpPathX(0) , tmpPathY(0) , tmpPathD(0)
Dim PathX%(0) , PathY%(0)

Dim DirX%(8) , DirY%(8), DirN(8,8)
For t=1 To 8
   Read DirX(t),DirY(t)
Next
For t=1 To 8
   For d=1 To 8
      Read DirN(t,d)
   Next
Next
Data 1,0, 0,1, -1,0, 0,-1, 1,1, -1,1, -1,-1, 1,-1
Data 1,2,4,5,8,3,6,7 , 1,2,3,5,6,0,0,0 , 2,3,4,6,7,0,0,0 , 1,3,4,7,8,0,0,0
Data 1,2,5,6,8,0,0,0 , 2,3,5,6,7,0,0,0 , 3,4,6,7,8,0,0,0 , 1,4,5,7,8,0,0,0

Function FindPath(EndX,EndY , StartX,StartY)
   If (StartX = EndX) And (StartY = EndY) Return 0

   Dim PathDir(MapWidth , MapHeight)
   
   tmpPathX(0) = StartX
   tmpPathY(0) = StartY
   tmpPathD(0) = 1
   dCnt = 8
   Repeat
      NewPathCnt = CurrentPathCnt
      For c = OldPathCnt To CurrentPathCnt
         For d=1 To dCnt
            Dir = DirN(tmpPathD(c) , d)
            x = tmpPathX(c) + DirX(Dir)
            y = tmpPathY(c) + DirY(Dir)
            If (x >= 0) And (x < MapWidth) And (y >= 0) And (y < MapHeight)
               If (PathDir(x,y) = 0) And (Map(x,y) = 0)
                  If Not ( (Dir>4 And (Map(tmpPathX(c) , y)<>0 Or Map(x , tmpPathY(c)) <> 0)) )
                     PathDir(x , y) = Dir
                     If (x = EndX) And (y = EndY)
                        Repeat
                           Dir = PathDir(EndX , EndY)
                           EndX = EndX - DirX(Dir)
                           EndY = EndY - DirY(Dir)
                           PathX(PathLength) = EndX
                           PathY(PathLength) = EndY
                           PathLength = PathLength + 1
                        Until (EndX = StartX) And (EndY = StartY)
                        Return PathLength - 1
                     EndIf
                     NewPathCnt = NewPathCnt + 1
                     tmpPathX(NewPathCnt) = x
                     tmpPathY(NewPathCnt) = y
                     tmpPathD(NewPathCnt) = Dir
                  EndIf
               EndIf
            EndIf
         Next
      Next
      If NewPathCnt = CurrentPathCnt Return -1
      dCnt = 5
      OldPathCnt = CurrentPathCnt+1
      CurrentPathCnt = NewPathCnt
   Forever
End Function

Function InitMap(width , height)
   MapWidth = width
   MapHeight = height
   Dim Map(MapWidth , MapHeight)
   Dim PathX(MapWidth * MapHeight) , PathY(MapWidth * MapHeight)
   Dim tmpPathX(MapWidth * MapHeight) , tmpPathY(MapWidth * MapHeight)   , tmpPathD(MapWidth * MapHeight)
End Function

Include "FindPath.bb"
SeedRnd MilliSecs()

Graphics 600,600,16,2
SetBuffer BackBuffer()

Const MapCellSize=4

InitMap(150,150)

Global StartX=0
Global StartY=0
Global EndX=(MapWidth-1 )
Global EndY=(MapHeight-1)

While Not KeyHit(1)

   Color 255,255,255
   For y=0 To MapHeight-1
   For x=0 To MapWidth-1
      If Map(x,y)<>0 Rect x * MapCellSize , y * MapCellSize , MapCellSize , MapCellSize
   Next
   Next
 
   MouseGridX=Floor(MouseX() / MapCellSize) : If MouseGridX > MapWidth-1  MouseGridX = MapWidth-1
   MouseGridY=Floor(MouseY() / MapCellSize) : If MouseGridY > MapHeight-1 MouseGridY = MapHeight-1

   If KeyDown(57)
      If (oldX <> MouseGridX Or oldY <> MouseGridY)
         oldX=MouseGridX
         oldY=MouseGridY
         map( MouseGridX , MouseGridY ) = Not map( MouseGridX , MouseGridY )
      EndIf   
   Else
      If MouseDown(1)
         StartX = MouseGridX
         StartY = MouseGridY
      EndIf
      If MouseDown(2)
         EndX = MouseGridX
         EndY = MouseGridY
      EndIf
   EndIf   

   ms = MilliSecs()
   PathLength=FindPath(StartX,StartY, EndX,EndY)
   AppTitle "Map: " + MapWidth + " x " + MapHeight + "  " + (MilliSecs() - ms) + " ms "
   If PathLength >= 0
      Color 0,0,255
      For t=0 To PathLength
         Rect (PathX(t) * MapCellSize) , (PathY(t) * MapCellSize) , MapCellSize , MapCellSize
      Next
   EndIf
   
   Color 0,255,0 :   Rect StartX * MapCellSize,StartY * MapCellSize , MapCellSize , MapCellSize
   Color 255,0,0 :   Rect EndX   * MapCellSize,EndY   * MapCellSize , MapCellSize , MapCellSize   
   
   Color 255,255,255
   Text 0, 0 , "Linke  Maustaste - Startpunkt setzen"
   Text 0,15 , "Rechte Maustaste - Endpunkt setzen"
   Text 0,30 , "Leertaste + Maus - Wand zeichnen"   
      
   Flip 0
   Cls
Wend
End