##	Author D. Urbano
##	Required:
##	The Tree is represented by a pointer to a element with 2 Octas: HEIGHT, root
##	root is a pointer to the top node of the tree
##	Nodes consists of 3 Octas: LeftChildPointer, RightChildPointer, Value
##	the last 3 bits of the LeftChildPointer are abused as the balance indicator


				PREFIX :node:insert:

HEIGHT			IS		0
ROOT			IS		8
NODE_SIZE		IS		24

VALUE			IS		16
KEY				IS		VALUE
LLINK			IS		0
RLINK			IS		8

B_MASK			IS		7
B_BALANCE		IS		2


HEAD			IS		$0		#received as param tree-pointer
K				IS		HEAD+1	#received as param key/value to insert/search
T				IS		K+1
P				IS		T+1
S				IS		P+1
Q				IS		S+1
R				IS		Q+1
B				IS		R+1
tmp				IS		B+1
a				IS		tmp+1

##			A1	Initialize					##
:node:insert:	IS		@
				LDO		P,HEAD,ROOT
				BZ		P,build_head
				SET		T,HEAD
				SET		S,P
				JMP		Compare_A2

##			Combination						##
##			A3	Move Left					##
##			A4	Move Right					##
Move_A3a4		IS		@
				LDO		Q,P,tmp
				ANDN	Q,Q,B_MASK
				BZ		Q,Insert_A5
				LDO		B,Q,LLINK
				AND		B,B,B_MASK			#filter balance factor
				CMP		B,B,B_BALANCE
				CSNZ	T,B,P
				CSNZ	S,B,Q
				SET		P,Q

##			A2	Compare						##
Compare_A2		IS		@
				LDO		tmp,P,KEY
				CMP		a,K,tmp
				ZSP		tmp,a,RLINK
				PBNZ	a,Move_A3a4
				SET		$0,P				#element found
				POP		1,0

##			just for the head				##
build_head		IS		@
				SET		tmp+2,NODE_SIZE
				GET		tmp,:rJ
				PUSHJ	tmp+1,:malloc:
				PUT		:rJ,tmp
				SET		tmp,0
				STO		K,tmp+1,KEY
				STO		tmp,tmp+1,RLINK
				SET		tmp,B_BALANCE
				STO		tmp,tmp+1,LLINK
				STO		tmp+1,HEAD,ROOT
				SET		tmp,1
				STO		tmp,HEAD,HEIGHT
				POP		0,0
				

##			A5	Insert						##
Insert_A5		IS		@
				GET		tmp+1,:rJ
				SET		tmp+3,NODE_SIZE
				PUSHJ	tmp+2,:malloc:
				PUT		:rJ,tmp+1
				STO		K,tmp+2,KEY
				SET		tmp+1,0
				STO		tmp+1,tmp+2,RLINK
				SET		tmp+1,B_BALANCE
				STO		tmp+1,tmp+2,LLINK
				LDO		B,P,tmp
				AND		B,B,B_MASK				## keep balance factor if inserted left
				OR		tmp+2,tmp+2,B
				STO		tmp+2,P,tmp
				SET		Q,tmp+2
				
##			A6	Adjust balance factors		##
Adj_Bal_fac		IS		@
				LDO		tmp,S,KEY
				CMP		tmp,tmp,K
				ZSNP	tmp,tmp,RLINK
				CSZ		tmp,tmp,LLINK
				LDO		P,S,tmp
				SET		R,P
ABf_loop		IS		@
				CMP		tmp,P,Q
				BZ		tmp,Balance_act
				LDO		tmp,P,KEY
				CMP		tmp,tmp,K
				BZ		tmp,Balance_act
				ZSN		B,tmp,B_BALANCE
				SUB		B,B,1
				LDO		tmp,P,LLINK
				ADD		tmp,tmp,B
				STO		tmp,P,LLINK
				ZSP		tmp,B,RLINK
				CSN		tmp,B,LLINK
				LDO		P,P,tmp
				JMP		ABf_loop

##			A7	Balance act.				##
Balance_act		IS		@
				LDO		tmp,S,KEY
				CMP		tmp,K,tmp
				ZSN		a,tmp,(B_BALANCE-1)
				CSZ		a,a,(B_BALANCE+1)
				LDO		a+1,S,LLINK
				AND		B,a+1,B_MASK
				CMP		tmp,B,a
				BZ		tmp,Rotation_Decision

				CMP		tmp,B,B_BALANCE
				ZSZ		B,tmp,a
				CSZ		B,B,B_BALANCE
				ANDN	a+1,a+1,B_MASK
				OR		a+1,a+1,B
				STO		a+1,S,LLINK
				PBNZ	tmp,A7_Terminate
				LDO		tmp,HEAD,HEIGHT
				ADD		tmp,tmp,1
				STO		tmp,HEAD,HEIGHT

A7_Terminate	IS		@
				POP		0,0

Rotation_Decision	IS	@
				LDO		B,R,LLINK
				AND		B,B,B_MASK
				CMP		tmp,B,a
				BNZ		tmp,Double_Rotation

##			A8	Single Rotation				##
Single_Rotation	IS		@
				SET		P,R
				CMP		tmp,a,B_BALANCE
				ZSNN	a+1,tmp,RLINK
				CSNZ	a+2,a+1,LLINK
				CSZ		a+2,a+1,RLINK
				CSZ		a+1,a+1,LLINK
				LDO		tmp,R,a+2
				ANDN	tmp,tmp,B_MASK
				STO		tmp,S,a+1
				ANDN	tmp,S,B_MASK
				STO		tmp,R,a+2
				LDO		tmp,S,LLINK
				ANDN	tmp,tmp,B_MASK
				OR		tmp,tmp,B_BALANCE
				STO		tmp,S,LLINK
				LDO		tmp,R,LLINK
				ANDN	tmp,tmp,B_MASK
				OR		tmp,tmp,B_BALANCE
				STO		tmp,R,LLINK
				JMP		Finishing

##			A9	Double Rotation				##
Double_Rotation	IS		@
				CMP		tmp,a,B_BALANCE
				ZSNN	a+1,tmp,RLINK		# a+1 -> a
				CSNZ	a+2,a+1,LLINK		# a+2 -> -a
				CSZ		a+2,a+1,RLINK
				CSZ		a+1,a+1,LLINK
				LDO		B,P,LLINK
				AND		B,B,B_MASK
				LDO		P,R,a+2
				LDO		tmp,P,a+1
				ANDN	tmp,tmp,B_MASK
				STO		tmp,R,a+2
				ANDN	tmp,R,B_MASK
				STO		tmp,P,a+1
				LDO		tmp,P,a+2
				ANDN	tmp,tmp,B_MASK
				STO		tmp,S,a+1
				ANDN	tmp,S,B_MASK
				STO		tmp,P,a+2
				CMP		tmp,B,a
				ZSZ		a+3,tmp,a
				CMP		tmp,B,B_BALANCE
				SUB		a+4,a,B_BALANCE
				NEG		a+4,a+4
				ADD		a+4,a+4,B_BALANCE
				ZSNZ	a+4,tmp,a+4
				LDO		tmp,S,LLINK
				ANDN	tmp,tmp,B_MASK
				OR		a+3,tmp,a+3
				STO		a+3,S,LLINK
				LDO		tmp,R,LLINK
				ANDN	tmp,tmp,B_MASK
				OR		a+4,tmp,a+4
				STO		a+4,R,LLINK
				LDO		B,P,LLINK
				ANDN	B,B,B_MASK
				OR		B,B,B_BALANCE
				STO		B,P,LLINK

##			A10	Finishing Touch				##
Finishing		IS		@
				LDO		tmp,T,RLINK
				ANDN	tmp,tmp,B_MASK
				ANDN	S,S,B_MASK
				CMP		tmp,tmp,S
				ZSZ		tmp,tmp,RLINK
				CSZ		tmp,tmp,LLINK
				LDO		B,T,LLINK
				AND		B,B,B_MASK
				ANDN	P,P,B_MASK
				OR		B,P,B
				STO		B,T,tmp
				POP		0,0