pathos/src/main/kotlin/net/idylls/pathos/Pathfinding.kt

package net.idylls.pathos

import net.runelite.api.Point
import net.runelite.api.CollisionDataFlag
import net.runelite.api.Tile as RLTile

import kotlin.math.abs

typealias ScenePoint = Point
typealias CollisionFlags = Int

fun sceneCoords(t: RLTile) = ScenePoint(t.sceneLocation.getX(), t.sceneLocation.getY())

fun buildPath(
	backEdges: Map<ScenePoint, ScenePoint>,
	end: ScenePoint
): List<ScenePoint> {
	val list = mutableListOf<ScenePoint>()
	var current: ScenePoint? = end
	while (current != null) {
		list.add(current)

		current = backEdges.get(current)
	}

	list.reverse()

	return list
}

fun findShortestPathAll(
	from: ScenePoint,
	to: Set<ScenePoint>,
	collisionFlags: Array<IntArray>,
	tiles: Array<Array<RLTile?>>,
): List<ScenePoint>? {
	return to
		.mapNotNull({ to_ -> findPath(from, to_, collisionFlags, tiles) })
		.minByOrNull({ p ->
			// minimize zig-zaggedness (manhattan distance) when choosing
			// between multiple possible minimal paths
			p.windowed(2).fold(0, { acc, w ->
				val (a, b) = w

				acc + abs(a.x - b.x) + abs(a.y - b.y)
			}) + p.size
		 })
}

fun findPath(
	from: ScenePoint,
	to: ScenePoint,
	collisionFlags: Array<IntArray>,
	tiles: Array<Array<RLTile?>>,
): List<ScenePoint>? {

	// A basic breadth-first search, taking into account OSRS movement
	// rules

	val seen = mutableSetOf<ScenePoint>(from)
	val queue = ArrayDeque<ScenePoint>()
	queue.add(from)

	val backEdges = mutableMapOf<ScenePoint, ScenePoint>()

	while (!queue.isEmpty()) {
		val current = queue.removeFirst()

		if (current == to) {
			return buildPath(backEdges, current)
		}

		val unvisitedNeighbors = neighbors(
			current,
			{ sp ->
				if (
					(sp.getX() < 0)
					|| (sp.getY() < 0)
					|| (sp.getX() >= SCENE_WIDTH)
					|| (sp.getY() >= SCENE_WIDTH)
				) {
					return@neighbors null
				}

				if (tiles[sp.getX()][sp.getY()] == null) {
					return@neighbors null
				}

				return@neighbors collisionFlags[sp.getX()][sp.getY()]
			},
		).filter({ n -> !seen.contains(n) })

		for (n in unvisitedNeighbors) {
			backEdges.put(n, current)
			queue.add(n)
			seen.add(n)
		}
	}

	// If there is not a path to the requested tile, try to find the
	// nearest pathable tile.
	val dist = { sp: ScenePoint ->
		val dx = sp.getX() - to.getX()
		val dy = sp.getY() - to.getY()

		(dx * dx) + (dy * dy)
	}

	val candidates = ArrayList(backEdges.keys)
	candidates.add(from)

	val candidateDistances = candidates.map({ sp ->
		Pair(sp, dist(sp))
	})

	val minDistance = candidateDistances.minByOrNull({ c -> c.second })
	if (minDistance == null) {
		return null
	}

	val minimalCandidates = candidateDistances.filter({ c ->
		c.second == minDistance.second
	})

	val minPath = minimalCandidates.mapNotNull({ c ->
		buildPath(backEdges, c.first)
	}).minWithOrNull({ a, b -> a.size - b.size })

	return minPath
}

fun sceneX(t: RLTile) = t.sceneLocation.x
fun sceneY(t: RLTile) = t.sceneLocation.y

fun canMoveCardinal(
	from: CollisionFlags,
	to: CollisionFlags,
	direction: Direction,
): Boolean {
	if ((to and (
		CollisionDataFlag.BLOCK_MOVEMENT_FULL
		or CollisionDataFlag.BLOCK_MOVEMENT_OBJECT
	)) > 0) {
		return false
	}

	if ((from and direction.collisionFlag) > 0) {
		return false
	}

	return true
}

/**
 * the adjDir params should be the direction to move from the adjacent
 * tile into the diagonal tile. e.g. If you are trying to move south-west
 * and adj1 params the southern tile, adjDir1 should be west (because you
 * move south, then west)
 */
fun canMoveDiagonal(
	from: CollisionFlags,
	to: CollisionFlags,
	direction: Direction,
	adjOk1: Boolean,
	adjFlags1: CollisionFlags,
	adjDir1: Direction,
	adjOk2: Boolean,
	adjFlags2: CollisionFlags,
	adjDir2: Direction,
): Boolean {
	return (
		adjOk1
		and adjOk2
		and canMoveCardinal(adjFlags1, to, adjDir1)
		and canMoveCardinal(adjFlags2, to, adjDir2)
		and canMoveCardinal(from, to, direction)
	)
}

enum class Direction(
	val dx: Int,
	val dy: Int,
	val collisionFlag: CollisionFlags,
) {
	WEST(-1, 0, CollisionDataFlag.BLOCK_MOVEMENT_WEST),
	EAST(1, 0, CollisionDataFlag.BLOCK_MOVEMENT_EAST),
	SOUTH(0, -1, CollisionDataFlag.BLOCK_MOVEMENT_SOUTH),
	NORTH(0, 1, CollisionDataFlag.BLOCK_MOVEMENT_NORTH),
	SOUTH_WEST(-1, -1, CollisionDataFlag.BLOCK_MOVEMENT_SOUTH_WEST),
	SOUTH_EAST(1, -1, CollisionDataFlag.BLOCK_MOVEMENT_SOUTH_EAST),
	NORTH_WEST(-1, 1, CollisionDataFlag.BLOCK_MOVEMENT_NORTH_WEST),
	NORTH_EAST(1, 1, CollisionDataFlag.BLOCK_MOVEMENT_NORTH_EAST),
}

fun neighbors(
	current: ScenePoint,
	collisionFlags: (ScenePoint) -> CollisionFlags?,
): Sequence<ScenePoint> {
	val spd = { sp: ScenePoint, d: Direction ->
		ScenePoint(sp.getX() + d.dx, sp.getY() + d.dy)
	}

	val cf = { d: Direction -> collisionFlags(spd(current, d)) }

	val currentFlags = collisionFlags(current)!!

	data class CanMoveCardinal(
		val canMove: Boolean,
		val flags: CollisionFlags,
	)
	val checkCardinal = c@{ d: Direction ->
		val flags = when (val f = cf(d)) {
			null -> return@c null
			else -> f
		}

		return@c CanMoveCardinal(
			canMoveCardinal(currentFlags, flags, d),
			flags,
		)
	}

	val west = checkCardinal(Direction.WEST)
	val east = checkCardinal(Direction.EAST)
	val south = checkCardinal(Direction.SOUTH)
	val north = checkCardinal(Direction.NORTH)

	val checkDiagonal = c@{
		d: Direction,
		cmc1: CanMoveCardinal?,
		cmc1Dir: Direction,
		cmc2: CanMoveCardinal?,
		cmc2Dir: Direction,
		->
		if (cmc1 == null) {
			return@c false
		}

		if (cmc2 == null) {
			return@c false
		}

		val diagonalFlags = when (val f = cf(d)) {
			null -> return@c false
			else -> f
		}

		return@c canMoveDiagonal(
			currentFlags,
			diagonalFlags,
			d,
			cmc1.canMove,
			cmc1.flags,
			cmc1Dir,
			cmc2.canMove,
			cmc2.flags,
			cmc2Dir,
		)
	}

	val southWest = checkDiagonal(
		Direction.SOUTH_WEST,
		south, Direction.WEST,
		west, Direction.SOUTH,
	)
	val southEast = checkDiagonal(
		Direction.SOUTH_EAST,
		south, Direction.EAST,
		east, Direction.SOUTH,
	)
	val northWest = checkDiagonal(
		Direction.NORTH_WEST,
		north, Direction.WEST,
		west, Direction.NORTH,
	)
	val northEast = checkDiagonal(
		Direction.NORTH_EAST,
		north, Direction.EAST,
		east, Direction.NORTH,
	)

	val e = { b: Boolean, d: Direction ->
		if (b) { spd(current, d) } else { null }
	}
	return sequenceOf(
		e(west?.canMove ?: false, Direction.WEST),
		e(east?.canMove ?: false, Direction.EAST),
		e(south?.canMove ?: false, Direction.SOUTH),
		e(north?.canMove ?: false, Direction.NORTH),
		e(southWest, Direction.SOUTH_WEST),
		e(southEast, Direction.SOUTH_EAST),
		e(northWest, Direction.NORTH_WEST),
		e(northEast, Direction.NORTH_EAST),
	).filter({ n -> n != null }).map({ n -> n!! })
}

fun runningPath(tiles: List<ScenePoint>): List<ScenePoint> {
	val list = mutableListOf<ScenePoint>()

	var current: Int = 0
	while (current < tiles.size) {
		list.add(tiles[current])
		current += 2
	}
	if ((tiles.size > 1) && ((tiles.size % 2) == 0)) {
		list.add(tiles.last())
	}

	return list
}