////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2017 Clayton Breckel
////////////////////////////////////////////////////////////////////////////////

package com.brekcel.RLGdx.map.pathfinding

import com.brekcel.RLGdx.map.Map
import com.brekcel.RLGdx.misc.Direction
import java.util.*


/** A* algorithm for the Map class */
class AStar : PathFinder {
	/** The cost to move horizontally/vertically */
	var normalCost: Int
	/** The cost to move diagonally */
	var diagCost: Int

	/** Creates an A* object
	 * @param map The map object that this A* works for
	 * @param normalCost The [normalCost]
	 *@param diagCost The [diagCost]
	 */
	constructor(map: Map, normalCost: Int, diagCost: Int) : super(map) {
		this.normalCost = normalCost
		this.diagCost = diagCost
	}

	/** The heuristic from source to dest */
	private fun heuristic(source: Point, destination: Point): Int {
		val dx = Math.abs(destination.x - source.y)
		val dy = Math.abs(destination.y - source.y)
		val diagonal = Math.min(dx, dy)
		val orthogonal = dx + dy - 2 * diagonal
		return diagonal * diagCost + orthogonal * normalCost
	}

	/** Get's a queue of directions from sx,sy to dx,dy
	 * @param sx The source x coord
	 * @param sy The source y coord
	 * @param dx The destination x coord
	 * @param dy The destination y coord
	 */
	fun getQueue(sx: Int, sy: Int, dx: Int, dy: Int): LinkedList<Direction> {
		val openList = PriorityQueue<Node>()
		val closedList = hashMapOf<Point, Cost>()
		var solution: Node? = null
		val start = Point(sx, sy)
		val dest = Point(dx, dy)
		openList.add(Node(null, start, Cost(0, heuristic(Point.EMPTY, dest))))

		fun addNeighbors(node: Node) {
			(-1).rangeTo(1).forEach { x ->
				(-1).rangeTo(1).forEach y@ { y ->
					if (x == 0 && y == 0) return@y
					val newPos = Point(node.position.x + x, node.position.y + y)
					if (!map.inBounds(newPos.x, newPos.y) || !map.canWalk(newPos.x, newPos.y)) return@y
					val cost = node.cost.distTravelled + if (x == 0 || y == 0) normalCost else diagCost
					openList.add(Node(node, newPos, Cost(cost, cost + heuristic(newPos, dest))))
				}
			}
		}

		while (!openList.isEmpty()) {
			val node = openList.poll()
			if (closedList.containsKey(node.position)) continue
			closedList.put(node.position, node.cost)
			if (node.position == dest) {
				solution = node
				break
			}
			addNeighbors(node)
		}
		val retList = LinkedList<Direction>()
		if (solution == null) {
			retList.add(Direction.ZERO)
			return retList
		}
		var node: Node = solution
		while (node.parent != null) {
			val pos = node.position - node.parent!!.position
			retList.add(Direction.getEnum(pos.x, pos.y))
			node = node.parent!!
		}
		return retList
	}

	/** Get's the direction that you need to be moving to reach dx,dy from sx,sy
	 * @param sx The source x coord
	 * @param sy The source y coord
	 * @param dx The destination x coord
	 * @param dy The destination y coord
	 */
	fun getDir(sx: Int, sy: Int, dx: Int, dy: Int) = getQueue(sx, sy, dx, dy).poll()

	/** A point */
	private data class Point(
			/** The x coord */
			var x: Int,
			/** The y coord */
			var y: Int) {
		/** The companion object */
		companion object {
			/** An empty Point. Point(0,0) */
			val EMPTY get() = Point(0, 0)
		}

		/** Subtract two points from each other */
		operator fun minus(other: Point) = Point(x - other.x, y - other.y)
	}

	/** A node for the A* */
	private data class Node(
			/** The parent node */
			var parent: Node?,
			/** This node's position */
			var position: Point,
			/** This node's cost */
			var cost: Cost) : Comparable<Node> {
		/** Compare two Nodes */
		override fun compareTo(other: Node) = cost.compareTo(other.cost)
	}

	/** The cost for the A* */
	data class Cost(
			/** The distance travelled for this node*/
			val distTravelled: Int,
			/** The total cost for this distance */
			val totalCost: Int) {
		/** Compare two Costs */
		fun compareTo(other: Cost) = totalCost.compareTo(other.totalCost)
	}
}