package simulation

import "core:fmt"
import "core:container/intrusive/list"
import rl "vendor:raylib"
import cm "../common"

MAX_TREE_SIZE : int : 16777216
MAX_TREE_DEPTH : int : 5
MAX_LEAF_SIZE : int : 64

OctreeNode :: struct {
	center: [3]cm.iFP,
	sector: OctantSector,
	entities: list.List,
	parent: ^OctreeNode,
	topNW: ^OctreeNode,
	topNE: ^OctreeNode,
	topSW: ^OctreeNode,
	topSE: ^OctreeNode,
	botNW: ^OctreeNode,
	botNE: ^OctreeNode,
	botSW: ^OctreeNode,
	botSE: ^OctreeNode,
}

OctantSector :: enum {
	TOPNW = 0b110,
	TOPNE = 0b111,
	TOPSW = 0b010,
	TOPSE = 0b011,
	BOTNW = 0b100,
	BOTNE = 0b101,
	BOTSW = 0b000,
	BOTSE = 0b001,
}

NodeEntity :: struct {
	value: int,
	node: list.Node
} 

octree: [dynamic]OctreeNode
sectorSize := [MAX_TREE_DEPTH]cm.iFP{858993459200,429496729600,214748364800,107374182400,53687091200}

InitOctree :: proc() {
	octree = make([dynamic]OctreeNode,0,MAX_TREE_SIZE)
	append(&octree,OctreeNode{})
	GrowTree(&octree[0],&octree)
}

DestroyOctree :: proc() {
	// dont forget to free linked lists
	delete(octree)
}

GrowTree :: proc(branch: ^OctreeNode, tree: ^[dynamic]OctreeNode, recursionDepth: int=0) {
	using cm

	nextNode := OctreeNode {
		parent = branch,
		center = branch.center,
	}
	halfLen := Div(sectorSize[recursionDepth],FOUR_METER)
	for sector in OctantSector {
		index := len(tree)
		append(tree,nextNode)
		switch sector {
			case OctantSector.TOPNW: {
				branch.topNW = &tree[index]
				tree[index].sector = OctantSector.TOPNW
				tree[index].center += [3]iFP{halfLen,halfLen,-halfLen}
			}
			case OctantSector.TOPNE: {
				branch.topNE = &tree[index]
				tree[index].sector = OctantSector.TOPNE
				tree[index].center += [3]iFP{halfLen,halfLen,halfLen}
			}
			case OctantSector.TOPSW: {
				branch.topSW = &tree[index]
				tree[index].sector = OctantSector.TOPSW
				tree[index].center += [3]iFP{-halfLen,halfLen,-halfLen}
			}
			case OctantSector.TOPSE: {
				branch.topSE = &tree[index]
				tree[index].sector = OctantSector.TOPSE
				tree[index].center += [3]iFP{-halfLen,halfLen,halfLen}
			}
			case OctantSector.BOTNW: {
				branch.botNW = &tree[index]
				tree[index].sector = OctantSector.BOTNW
				tree[index].center += [3]iFP{halfLen,-halfLen,-halfLen}
			}
			case OctantSector.BOTNE: {
				branch.botNE = &tree[index]
				tree[index].sector = OctantSector.BOTNE
				tree[index].center += [3]iFP{halfLen,-halfLen,halfLen}
			}
			case OctantSector.BOTSW: { 
				branch.botSW = &tree[index]
				tree[index].sector = OctantSector.BOTSW
				tree[index].center += [3]iFP{-halfLen,-halfLen,-halfLen}
			}
			case OctantSector.BOTSE: { 
				branch.botSE = &tree[index]
				tree[index].sector = OctantSector.BOTSE
				tree[index].center += [3]iFP{-halfLen,-halfLen,halfLen}
			}
		}
		if recursionDepth < MAX_TREE_DEPTH-2 do GrowTree(&tree[index], tree, recursionDepth+1)
	}
}


FindEnclosingNode :: proc(box: AABB, point: [3]cm.iFP, tree: ^[dynamic]OctreeNode) -> ^OctreeNode {
	b := AABB{
		max = box.max+point,
		min = box.min+point,
	}
	min_leaf := FindLeaf(b.min, tree)
	max_leaf := FindLeaf(b.max, tree)

	return FindCommonNode(min_leaf,max_leaf,tree)
}

FindCommonNode :: proc(leaf1: ^OctreeNode, leaf2: ^OctreeNode, tree: ^[dynamic]OctreeNode) -> ^OctreeNode {
	if leaf1 == leaf2 do return leaf1
	if leaf1.parent == nil do return leaf1
	if leaf2.parent == nil do return leaf2
	branch1 := leaf1.parent
	branch2 := leaf2.parent

	for _ in 0..<MAX_TREE_DEPTH {
		if branch1 == branch2 do return branch1
		if branch1.parent == nil do return branch1
		if branch2.parent == nil do return branch2
		branch1 = branch1.parent
		branch2 = branch2.parent
	}

	return nil
}

FindLeaf :: proc(point: [3]cm.iFP, tree: ^[dynamic]OctreeNode) -> ^OctreeNode {
	currentBranch := &tree[0]

	for _ in 0..<MAX_TREE_DEPTH {
		nextNode: ^OctreeNode
		switch FindOctant(point,currentBranch.center) {
			case OctantSector.TOPNW: { nextNode = currentBranch.topNW }
			case OctantSector.TOPNE: { nextNode = currentBranch.topNE }
			case OctantSector.TOPSW: { nextNode = currentBranch.topSW }
			case OctantSector.TOPSE: { nextNode = currentBranch.topSE }
			case OctantSector.BOTNW: { nextNode = currentBranch.botNW }
			case OctantSector.BOTNE: { nextNode = currentBranch.botNE }
			case OctantSector.BOTSW: { nextNode = currentBranch.botSW }
			case OctantSector.BOTSE: { nextNode = currentBranch.botSE }
		}
		
		if nextNode.topNW == nil do return nextNode
		else do currentBranch = nextNode

	}
	return nil
}

AddEntityToNode :: proc(entity: int, node: ^OctreeNode) -> bool {
	newNode := new(NodeEntity)
	newNode.value = entity

	return UniqueAppend(&node.entities, newNode)
}

RemoveEntityFromNode :: proc(entity: int, node: ^OctreeNode) -> bool {
	newNode: NodeEntity
	newNode.value = entity

	return ListRemove(&node.entities, &newNode)
}

FindOctant :: proc(point: [3]cm.iFP, cubeCenter: [3]cm.iFP) -> OctantSector {
    isTop := point[1] > cubeCenter[1]
    isNorth := point[0] > cubeCenter[0]
    isWest := point[2] < cubeCenter[2]

    if isTop {
        if isNorth {
            if isWest do return .TOPNW
            else do return .TOPNE
        } else {
            if isWest do return .TOPSW
            else do return .TOPSE
        }
    } else {
        if isNorth {
            if isWest do return .BOTNW
            else do return .BOTNE
        } else {
            if isWest do return .BOTSW
            else do return .BOTSE
        }
    }
}

IterateTree :: proc(caller: int, delta: cm.iFP, function: proc(e1: int, e2: int, delta: cm.iFP), node: ^OctreeNode) {
	if !list.is_empty(&node.entities) {
		it := list.iterator_head(node.entities, NodeEntity, "node")
		for e in list.iterate_next(&it) {
			if e.value == caller do continue
			function(caller,e.value,delta)
		}
	}
	if node.topNW == nil do return
	for sector in OctantSector {
		switch sector {
			case OctantSector.TOPNW: {
				IterateTree(caller, delta, function, node.topNW)
			}
			case OctantSector.TOPNE: {
				IterateTree(caller, delta, function, node.topNE)
			}
			case OctantSector.TOPSW: {
				IterateTree(caller, delta, function, node.topSW)
			}
			case OctantSector.TOPSE: {
				IterateTree(caller, delta, function, node.topSE)
			}
			case OctantSector.BOTNW: {
				IterateTree(caller, delta, function, node.botNW)
			}
			case OctantSector.BOTNE: {
				IterateTree(caller, delta, function, node.botNE)
			}
			case OctantSector.BOTSW: { 
				IterateTree(caller, delta, function, node.botSW)
			}
			case OctantSector.BOTSE: { 
				IterateTree(caller, delta, function, node.botSE)
			}
		}
	}
}

DebugHighlightSector :: proc(tree: ^[dynamic]OctreeNode) {
	for e in tree {
		ent := e
		if !list.is_empty(&ent.entities) {
			nodeDepth: int = 0 
			for i in 0..<MAX_TREE_DEPTH {
				nodeDepth = i
				if ent.parent == nil do break
				ent = ent.parent^
			}
			center := e.center
			cubeSize := IntToFloat(sectorSize[nodeDepth])
			rl.DrawCubeWires(ToFloatVector(center), cubeSize, cubeSize, cubeSize, rl.RED)
		}
	}
}