package main

import (
	"container/heap"
	"math"
)

const cellSize = 25.

type PFBox struct {
	x1		float64
	x2		float64
	y1		float64
	y2		float64
}

func (b *PFBox) width() float64 {
	return b.x2 - b.x1
}

func (b *PFBox) height() float64 {
	return b.y2 - b.y1
}

func (b *PFBox) contains(x float64, y float64) bool {
	return (b.x1 < x && x < b.x2 && b.y1 < y && y < b.y2)
}

func (b *PFBox) intersects(box *PFBox) bool {
	return (b.x1 <= box.x2 && box.x1 <= b.x2 && b.y1 <= box.y2 && box.y1 <= b.y2)
}

func CreateSquarePFBox(cx float64, cy float64, side float64) *PFBox {
	halfSide := side / 2
	return &PFBox{x1: cx - halfSide, y1: cy - halfSide, x2: cx + halfSide, y2: cy + halfSide}
}

type PFNodeTree struct {
	x		float64
	y		float64
	level		int
	listId		int
	heuristic	float64
	region		*PFBox
	root		*PFNodeTree
	crossable	bool
	isLeaf		bool
	quads		[]*PFNodeTree
	neighbors	[]*PFNodeTree

	heapInx		int
	pathParent	*PFNodeTree
}

func CreatePFNodeTree(region *PFBox, obstacles []*PFBox, root *PFNodeTree, level int) *PFNodeTree {
	n := PFNodeTree{}
	n.quads = make([]*PFNodeTree, 4)
	n.level = level

	if root != nil {
		n.root = root
	} else {
		n.root = &n
	}

	n.region = region
	n.x = (region.x1 + region.x2) / 2
	n.y = (region.y1 + region.y2) / 2

	n.obstacles = obstacles

	n.crossable = true
	n.isLeaf = false

	if len(n.obstacles) == 0 {
		n.isLeaf = true
	} else if n.level == 0 {
		n.isLeaf = true
		n.crossable = false
	}

	return &n
}

func (n *PFNodeTree) getQuad(x float64, y float64) *PFNodeTree {
	if x < n.x && y < n.y {
		return n.quads[0]
	} else {
		if x >= n.x && y < n.y {
			return n.quads[1]
		} else if x < n.x && y >= n.y {
			return n.quads[2]
		}
	}

	return n.quads[3]
}

func (n *PFNodeTree) get(x float64, y float64) *PFNodeTree {
	if !n.region.contains(x, y) {
		return nil
	}

	if n.isLeaf {
		return n
	}

	return n.getQuad(x, y).get(x, y)
}

func (n *PFNodeTree) update() {
	if n.isLeaf {
		return
	}

	l := n.region.x1
	r := n.region.x2
	t := n.region.y1
	b := n.region.y2

	x := n.x
	y := n.y

	subregions := []*PFBox{
		&PFBox{x1: l, y1: t, x2: x, y2: y},
		&PFBox{x1: x, y1: t, x2: r, y2: y},
		&PFBox{x1: l, y1: y, x2: x, y2: b},
		&PFBox{x1: x, y1: y, x2: r, y2: b},
	}

	for i, subregion := range subregions {

		var subregionObstacles []*PFBox

		for _, obstacle := range n.obstacles {
			if subregion.intersects(obstacle) {
				subregionObstacles = append(subregionObstacles, obstacle)
			}

		}

		n.quads[i] = CreatePFNodeTree(subregion, subregionObstacles, n.root, n.level-1)
		n.quads[i].update()
	}
}

func (n *PFNodeTree) getNeighbors() []*PFNodeTree {
	if n.neighbors != nil {
		return n.neighbors
	}

	left := n.region.x1
	right := n.region.x2
	top := n.region.y1
	bottom := n.region.y2

	minCell := cellSize
	possibleNeighbors := math.Pow(2, float64(n.level))

	var neighbors PFNodeHeap

	for x := -(possibleNeighbors + 1); x <= (possibleNeighbors + 1); x += 2 {
		xx := n.x + minCell*(x/2)

		neighbor := n.root.get(xx, top-minCell/2)

		if neighbor != nil && neighbor.crossable && !neighbors.has(neighbor) {
			neighbors = append(neighbors, neighbor)
		}

		neighbor = n.root.get(xx, bottom+minCell/2)

		if neighbor != nil && neighbor.crossable && !neighbors.has(neighbor)  {
			neighbors = append(neighbors, neighbor)
		}
	}

	for y := -(possibleNeighbors - 1); y <= (possibleNeighbors - 1); y += 2 {
		yy := n.y + minCell*(y/2)

		neighbor := n.root.get(left-minCell/2, yy)
		if neighbor != nil && neighbor.crossable && !neighbors.has(neighbor) {
			neighbors = append(neighbors, neighbor)
		}

		neighbor = n.root.get(right+minCell/2, yy)
		if neighbor != nil && neighbor.crossable && !neighbors.has(neighbor) {
			neighbors = append(neighbors, neighbor)
		}

	}

	n.neighbors = neighbors

	return n.neighbors
}

type PFNodeHeap []*PFNodeTree

func (nn PFNodeHeap) has(n *PFNodeTree) bool {
	for _, c := range nn {
		if c == n {
			return true
		}
	}
	return false
}

func (h PFNodeHeap) Len() int           { return len(h) }

func (h PFNodeHeap) Less(i, j int) bool { return h[i].heuristic < h[j].heuristic }

func (h PFNodeHeap) Swap(i, j int)      {
	h[i].heapInx, h[j].heapInx = h[j].heapInx, h[i].heapInx
	h[i], h[j] = h[j], h[i] }

func (h *PFNodeHeap) Push(x interface{}) {
	item := x.(*PFNodeTree)
	item.heapInx = len(*h)
	*h = append(*h, x.(*PFNodeTree))
}

func (h *PFNodeHeap) Pop() interface{} {
	old := *h
	n := len(old)

	x := old[n-1]
	*h = old[0 : n-1]
	return x
}

func distance(a *PFNodeTree, b *PFNodeTree) float64 {
	x_dist := b.x - a.x
	y_dist := b.y - a.y
	return x_dist*x_dist + y_dist*y_dist
}

func (n *PFNodeTree) PathFind(p1 *Vec2, p2 *Vec2) []*Vec2 {
	unqiueListID += 2
	closedId := unqiueListID - 1
	openId := unqiueListID

	src := n.get(p1[0], p1[1])
	dst := n.get(p2[0], p2[1])
	if !dst.crossable {
		dst = dst.getCrossableNeighbor()
	}
       //Для того, чтобы монстр не ходил всегда в притирку со стенкой, я сделал препятствия
       //шире на ~20px, поэтому иногда игрок может стоять в "стенке"
       //Поэтому если конечная точка находится в стенке, мы идем к ближайшему соседу


	var traveled = make(map[*PFNodeTree]float64)
        //Посещенные за эту итерацию клетки вместе с их расстояниями


	src.listId = openId
	if src == dst {
		return []*Vec2{p2}
	} else {
		traveled[src] = 0
		src.pathParent = src
		dst.pathParent = nil
		src.heuristic = distance(src, dst)

		open := &PFNodeHeap{}
		heap.Init(open)
		heap.Push(open, src)
               //Инициализируем кучу, добавляем первую клетку

		for len(*open) > 0 {
			current := heap.Pop(open).(*PFNodeTree)
                    //В цикле берем лучшую на данный момент клетку и пытаемся улучшить ее путь
			if current == dst {
				break  //конечная клетка достигнута
			}

			current.listId = closedId

			var neighbors = current.getNeighbors()
                        //получаем список соседей
			for _, neighbor := range neighbors {
				if neighbor.listId == closedId {
					continue //пропускаем соседей обработанных за эту операцию
				}

				var oldPath float64 = math.Inf(1)
				tr, _ := traveled[neighbor]
				if tr > 0 {
					oldPath = tr
				}
                             //Предыдущий путь равен либо значению traveled для  этой клетки, а если его нет
                             // то присваиваем бесконечности
				var replacedPath float64 = -1

					newPath := traveled[current] + distance(current, neighbor)
                                  //Пересчитываем новый путь попадания в соседа, 
                                  //равный путь к текущей клетке + расстояние к соседу
				if newPath < oldPath {
                                  //Если путь короче, то перезаписываем родительскую клетку
                                  //эвристическое расстояние
					traveled[neighbor] = newPath
					neighbor.pathParent = current
					neighbor.heuristic = newPath + distance(neighbor, dst)
                                  // Если сосед есть в списке, то обновляем его место в массиве методом Fix
                                  // Иначе, добавляем новый элемент
					if neighbor.listId == openId {
						heap.Fix(open, neighbor.heapInx)
					} else {
						heap.Push(open, neighbor)
						neighbor.listId = openId
					}
				    }
				}
			}
		}
	}

	if dst.pathParent == nil {
		return []*Vec2{p2} //Путь заблокирован
	}

	var pathNodes []*PFNodeTree = make([]*PFNodeTree, 0)

	var node = dst

	for node != src {
		pathNodes = append([]*PFNodeTree{node}, pathNodes...)

		node = node.pathParent
		if node == nil {
			break
		}
	}

        //Сглаживаем путь, об этом ниже
	return n.lerpPath(pathNodes)
}