My own implementation to the A* pathfinding algorithm, written in Kotlin.

Main.kt

import kotlin.math.abs


const val width = 59
const val height = 80

fun main() {
  val start = Coord(0, 3)
  val end = Coord(3, 4)
  val obstacles = listOf(
    Coord(2, 2),
    Coord(2, 3),
    Coord(2, 4),
    Coord(1, 2),
    Coord(3, 1),
    Coord(3, 2)
  )

  var nProcessings = 0

  findPath(
    start,
    end,
    obstacles,
    onNodeProcessed = { nProcessings++ },
    onDone = {
      printParenting(start, end, it, obstacles)
      println("performed $nProcessings processings.")
    }
  )
}

fun findPath(
  start: Coord,
  end: Coord,
  obstacles: List<Coord> = listOf(),
  onUpdateCurrentExploringNode: (Node) -> Unit = {},
  onOpenListInsertion: (Node) -> Unit = {},
  onOpenListRemove: (Node) -> Unit = {},
  onOpenListSort: (List<Node>) -> Unit = {},
  onClosedListInsertion: (Node) -> Unit = {},
  onNodeProcessed: (Node) -> Unit = {},
  onDone: (Node) -> Unit = {}
): Node {
  val open = mutableListOf<Node>()
  val closed = mutableListOf<Node>()
  var currExploringNode = Node(start)
  onUpdateCurrentExploringNode(currExploringNode)

  open += currExploringNode
  onOpenListInsertion(currExploringNode)

  while (true) {
    if (currExploringNode.coord.x == end.x && currExploringNode.coord.y == end.y) {
      onDone(currExploringNode)
      break
    }

    val neighbors = currExploringNode.getNeighbors()
    neighbors.forEach { neighbor ->
      if (obstacles.any { it.x == neighbor.coord.x && it.y == neighbor.coord.y }) {
        neighbor.blocked = true
      }

      if (!neighbor.blocked) {
        if (!open.containsNode(neighbor) && !closed.containsNode(neighbor)) {
          neighbor.processCosts(start, end)
          onNodeProcessed(neighbor)

          open += neighbor
          onOpenListInsertion(neighbor)
        }
      }
    }

    closed += currExploringNode
    onClosedListInsertion(currExploringNode)

    open -= currExploringNode
    onOpenListRemove(currExploringNode)

    open.sortBy { it.f }
    onOpenListSort(open)

    currExploringNode = open.first()
    onUpdateCurrentExploringNode(currExploringNode)
  }

  return currExploringNode
}

/**
 * Just to debug.
 */
fun printParenting(
  start: Coord,
  end: Coord,
  lastNode: Node,
  obstacles: List<Coord> = listOf()
) {
  val empty = "_"
  val waypoint = "."
  val blocked = "#"

  val table = Array(height) { Array(width) { empty } }
  var tmp = lastNode
  while (tmp.parent != null) {
    table[tmp.coord.y][tmp.coord.x] = waypoint
    tmp = tmp.parent!!
  }

  table[start.y][start.x] = "!"
  table[end.y][end.x] = "X"

  obstacles.forEach {
    table[it.y][it.x] = blocked
  }

  table.forEach { a ->
    a.forEach { b ->
      print("$b  ")
    }
    println()
  }
}

/**
 * Directly extends a list with [Node] items.
 */
fun List<Node>.containsNode(node: Node) =
  this.any { it.coord.x == node.coord.x && it.coord.y == node.coord.y }

/**
 * Represents a bidimensional coordinate inside a space.
 */
data class Coord(val x: Int, val y: Int) {

  fun inBounds(w: Int, h: Int) =
    (x in 0 until w) && (y in 0 until h)
}

/**
 * Represents each point inside a map graph.
 */
data class Node(val coord: Coord) {
  var parent: Node? = null
  var f: Int = 0
  var blocked = false

  private var g: Int = 0
  private var h: Int = 0

  fun processCosts(start: Coord, end: Coord) {
    // not considering diagonals
    g = abs(coord.x - start.x) + abs(coord.y - start.y)
    h = abs(end.x - start.x) + abs(end.y - start.y)
    f = g + h
  }

  fun getNeighbors(): MutableList<Node> {
    val res = mutableListOf<Node>()

    // not considering diagonals
    val n = Coord(coord.x, coord.y + 1)
    val s = Coord(coord.x, coord.y - 1)
    val e = Coord(coord.x + 1, coord.y)
    val w = Coord(coord.x - 1, coord.y)

    if (n.inBounds(width, height)) res += Node(n)
    if (s.inBounds(width, height)) res += Node(s)
    if (e.inBounds(width, height)) res += Node(e)
    if (w.inBounds(width, height)) res += Node(w)

    res.forEach { it.parent = this }

    return res
  }
}