FindPath:
    ld iy, PathFindingData
    ld (iy+PFStartX), 2
    ld (iy+PFStartY), 4
    ld (iy+PFEndX), 6
    ld (iy+PFEndY), 5
    ld (iy+PFAmountOfOpenTiles), 1
    ld (iy+PFAmountOfClosedTiles), 0
    ld hl, PFOpenedList
    ld (hl), 2
    inc hl
    ld (hl), 4
    inc hl
    ld (hl), 0
    inc hl
    ld (hl), 5-4+6-2
    jr FindPath2
TempMapData:
    .db 0, 0, 0, 0, 0, 0, 0, 0, 0
    .db 0, 1, 1, 1, 1, 1, 1, 1, 0
    .db 0, 1, 1, 1, 0, 1, 1, 1, 0
    .db 0, 1, 1, 1, 0, 1, 1, 1, 0
    .db 0, 1, 1, 1, 0, 1, 1, 1, 0
    .db 0, 1, 0, 1, 0, 1, 1, 1, 0
    .db 0, 1, 1, 1, 1, 1, 1, 1, 0
    .db 0, 0, 0, 0, 0, 0, 0, 0, 0
FindPath2:
; Get the minimum score
    ld ix, PFOpenedList-4
    xor a, a
    ld b, a                                                                     ; B = index of tile in open list
    ld (iy+PFIndexOfCurInOpenList), a
    dec a                                                                       ; A = current minimum score
GetMinimumTile:
    lea ix, ix+4
    ld c, (ix+3)                                                                ; C = score of tile in open list
    cp a, c
    jr c, +_
    ld a, c                                                                     ; The score is lower than the current score, so change the active tile and score
    ld (iy+PFIndexOfCurInOpenList), b
_:  inc b
    ld d, a                                                                     ; D = temp current minimum score
    ld a, b
    cp a, (iy+PFAmountOfOpenTiles)                                              ; Check if the index is equal to the amount of tiles in the open list
    ld a, d
    jr nz, GetMinimumTile
    ld e, (iy+PFIndexOfCurInOpenList)                                           ; Get the tile with the lowest score
    ld d, 4
    mlt de
    ld ix, PFOpenedList
    add ix, de
    ld hl, (ix)
    ld (iy+PFCurX), hl                                                          ; Copy the X, Y and depth of the current tile
    ld a, (iy+PFIndexOfCurInOpenList)
    ld b, (iy+PFAmountOfOpenTiles)
    dec b
    ld (iy+PFAmountOfOpenTiles), b
    sub a, b
    jr z, CheckLeftNeighbour                                                    ; The selected tile was the last in the list, so don't move the others
    lea de, ix                                                                  ; Move selected tile to closed list
    lea hl, ix+4
    ld c, a
    ld b, 4
    ldir
CheckLeftNeighbour:
    ld a, (iy+PFCurX)
    or a, a
    jr z, CheckUpperNeighbour
    dec a
    ld d, a
    ld e, (iy+PFCurY)
    call FindTileInBothLists
    call nz, CheckIfTileIsWalkable
    call nz, AddTileToOpenList
CheckUpperNeighbour:
    ld a, (iy+PFCurY)
    or a, a
    jr z, CheckRightNeighbour
    dec a
    ld e, a
    ld d, (iy+PFCurX)
    call FindTileInBothLists
    call nz, CheckIfTileIsWalkable
    call nz, AddTileToOpenList
CheckRightNeighbour:
    ld a, (iy+PFCurX)
    cp a, 8
    jr z, CheckLowerNeightbour
    inc a
    ld d, a
    ld e, (iy+PFCurY)
    call FindTileInBothLists
    call nz, CheckIfTileIsWalkable
    call nz, AddTileToOpenList
CheckLowerNeightbour:
    ld a, (iy+PFCurY)
    cp a, 7
    jr z, +_
    inc a
    ld e, a
    ld d, (iy+PFCurX)
    call FindTileInBothLists
    call nz, CheckIfTileIsWalkable
    call nz, AddTileToOpenList
_:  ret

FindTileInBothLists:
    ld hl, PFOpenedList
    ld b, (iy+PFAmountOfOpenTiles
    call FindTileInList
    ld hl, PFClosedList
    ld b, (iy+PFAmountOfClosedTiles)
FindTileInList:
; Input:
;   B = Amount of elements
;   D = X coordinate
;   E = Y coordinate
;  HL = Pointer to list
    ld a, b
    or a, a
    jr nz, FindTileInListLoop
    inc a
    ret
FindTileInListLoop:
    ld a, (hl)
    inc hl
    ld c, (hl)
    inc hl
    inc hl
    inc hl
    cp a, d
    jr nz, +_
    ld a, c
    cp a, e
    ret z
_:  djnz FindTileInListLoop
    ret

CheckIfTileIsWalkable:
    ld l, e
    ld h, 9
    mlt hl
    ld c, d
    ld b, 1
    mlt bc
    add hl, bc
    ld bc, TempMapData
    add hl, bc
    ld a, (hl)
    or a, a
    ret

AddTileToOpenList:
    ld c, (iy+PFAmountOfOpenTiles)
    ld b, 4
    mlt bc
    ld hl, PFOpenedList
    add hl, bc
    ld (hl), d
    inc hl
    ld (hl), e
    inc hl
    ld a, (iy+PFEndX)                                                           ; Find the distance to end, abs(X_end - X_curr) + abs(Y_end - Y_curr)
    cp a, d
    jr c, +_
    ld a, d
_:  ld b, a
    ld a, (iy+PFEndY)
    cp a, e
    jr c, +_
    ld a, e
_:  add a, b
    ld (hl), a
    inc hl
    ld a, (iy+PFCurTileDepth)
    inc a
    ld (hl), a
    inc (iy+PFAmountOfOpenTiles)
    ret