
	.include	"road.i"
	.include	"car.i"
	.include	"playfield.i"
	.include	"mem_macros.i"

;***********************************************************
;*
;* init_road (initialisieren eines spielfeldes) 
;*
;* r11  = ROAD_STRUCT
;*

	.global	init_road
	.type	init_road,function	
	
init_road

	stmfd	sp!,{r0-r12,lr}

	;**** set gfx

	mov 	r6,#$4000000   		;
	mov	r1,#$1e00		;mode 0 bg 1/2/3 + sprites
	strh 	r1,[r6]			;store

	;**** clr trp
	
	mov	r0,#0
	strh 	r0,[r6,#$50]	
	strh 	r0,[r6,#$52]	
	strh 	r0,[r6,#$54]	
	
	;**** mach dunkel
	
	mov	r0,#$ff
	strh	r0,[r6,#$50]
	mov	r0,#$1f	
	strh	r0,[r6,#$54]
	
	;**** init timer 1 irq for sound
	
	ldr	r0,=irq_mod
	mov	r1,#4			;timer 1			
	ldr	r2,=add_irq_th+1
	mov	lr,pc
	bx	r2
	
	;**** play sound
	
	ldr	r0,=mod
	bl	init_mod		;init_mod
	bl	start_mod		;start_mod

	;**** init PLAYFIELD_STRUCT
	
	mem_malloc_mc	membaseW_adr, #PF_LENGTH		;playfieldstruct
	
	str	r2,[r11,#R_PLAYFIELD]				;merke pointer und init struct

	mov	r3,#0				;x=0
	mov	r4,#0				;y=0
	ldr	r5,[r11,#R_MAP_1]		;map1 src
	ldr	r6,=($6000000+49152)	;map dest
	ldr	r7,[r11,#R_MAP_2]		;map2 src
	ldr	r8,=($6000000+51200)	;map dest
	mov	r9,#64*16			;breite
	mov	r10,#64*16			;höhe

	str	r3,[r2,#PF_X]		
	str	r4,[r2,#PF_Y]
	str	r5,[r2,#PF_BG2_SRC]		
	str	r6,[r2,#PF_BG2_DEST]	
	str	r7,[r2,#PF_BG3_SRC]		
	str	r8,[r2,#PF_BG3_DEST]	
	str	r9,[r2,#PF_WIDTH]	
	str	r10,[r2,#PF_HEIGHT]	

	;map 3

	ldr	r5,[r11,#R_MAP_3]		;map3 src
	ldr	r6,=($6000000+53248)		;map dest
	
	str	r5,[r2,#PF_BG1_SRC]		
	str	r6,[r2,#PF_BG1_DEST]
	
	;**** init gfx

	mov 	r6,#$4000000   		;
	ldr	r1,=$3883		;0 tiles / 18 map (49152) / 256 c prio 0	
	strh 	r1,[r6,#$c]	
		
	ldr	r1,=$3982		;0 tiles / 19 map (51200) / 256   prio 1
	strh 	r1,[r6,#$e]

	ldr	r1,=$3A82		;0 tiles / 1A map (53248) / 256   prio 1
	strh 	r1,[r6,#$a]

	;**** set scroll
	
	mov	r0,#0
	strh	r0,[r6,#$18]
	strh	r0,[r6,#$1a]
	strh	r0,[r6,#$1c]
	strh	r0,[r6,#$1e]

	;**** get 4116 bytes ram for lzssbuffer
	
	ldr	r0,=4116
	stack_alloc_mc	r0
	mov	r10,r1
	mov	r2,#4080
	bl	memclear32		;clear

	;***** kopiere gfx daten
	
	mem_malloc_mc	membaseW_adr, #49152		;get 
	mov	r12,r2		
				
	ldr	r0,[r11,#R_GFX]		;Source
	
	add	r0,r0,#12		;len
	ldr	r2,[r0],#4		;get len and goto source start (r0+4)
	add	r2,r2,r0		;len+source
	mov	r1,r12			;dest
	mov	r3,r10			;tbuffer
	ldr	r4,=lzss_dec
	mov	lr,pc
	bx	r4
	
	mov	r0,r12
	mov	r1,#$6000000	
	mov	r2,#(256*192)
	bl	dmacpy	
	
	mem_free_mc	membaseW_adr,r12		;free 

	;**** sprite daten
	
	;mem_malloc_mc	membaseW_adr, #8192		;get 8k
	;mov	r12,r2

	ldr	r0,[r11,#R_SPRITE]
	;mov	r1,r12	
	;swi	$110000					;decomp lz77 wram
	
	;mov	r0,r12
	ldr	r1,=$6010000
	ldr	r2,=8192
	bl	dmacpy	
	
	;mem_free_mc	membaseW_adr,r12		;free 32

	;**** free 4116
	
	stack_free_mc

	;**** setze MAPS/GFXS
		
	ldr	r0,[r11,#R_PAL]
	mov	r1,#$5000000	
	mov	r2,#(256*2)
	bl	dmacpy

	ldr	r0,[r11,#R_MAP_1]
	ldr	r1,[r11,#R_PLAYFIELD]	;playfield	
	ldr	r1,[r1,#PF_BG2_DEST]	;bg dest
	mov	r2,#128*2-(32*2)
	bl	copymap

	ldr	r0,[r11,#R_MAP_2]
	ldr	r1,[r11,#R_PLAYFIELD]	;playfield	
	ldr	r1,[r1,#PF_BG3_DEST]	;bg dest
	mov	r2,#128*2-(32*2)
	bl	copymap
	
	ldr	r0,[r11,#R_MAP_3]
	ldr	r1,[r11,#R_PLAYFIELD]	;playfield	
	ldr	r1,[r1,#PF_BG1_DEST]	;bg dest
	mov	r2,#128*2-(32*2)
	bl	copymap
	
	ldr	r0,[r11,#R_SPRITE_PAL]
	ldr	r1,=$5000200
	mov	r2,#16*2
	bl	dmacpy

	;**** init cars ****
	
	
	;**** geniere speicher für carstructs
	
	ldr	r12,=car_count
	ldr	r12,[r12]
	str	r12,[r11,#R_CARCOUNT]			;merke anzahl in roadstruct
	
	mov	r0,#CAR_STRUCT_LEN
	mul	r2,r0,r12				;*anzahl
	mem_malloc_mc	membaseW_adr, r2		;get mem
	str	r2,[r11,#R_CARS]			;merke pointer in roadstruct
	mov	r9,r2
	
	ldr	r8,=car_startpos
	mov	lr,#0
get_carsp	
	
	;** get new sprite
	
	ldr	r0,[r8],#4		;get startposx
	ldr	r1,[r8],#4		;get startposz
	
	str	r0,[r9,#CAR_X_POS]	;save x
	str	r1,[r9,#CAR_Y_POS]	;save y
	str	r0,[r9,#CAR_XE]		;save xend
	str	r1,[r9,#CAR_YE]		;save yend
	
	mov	r2,#0
	str	r2,[r9,#CAR_SPEED]	
	str	r2,[r9,#CAR_WINKEL]
	str	r2,[r9,#CAR_BGMODE]
	str	r2,[r9,#CAR_ANIMPHASE]
	str	r2,[r9,#CAR_ANIMPHASE_C]
	mov	r2,#$100
	str	r2,[r9,#CAR_ZOOM]
	mov	r2,#$c0			;Wegmarke
	str	r2,[r9,#CAR_LASTWMARK]
	mov	r2,#$2			;car status über allem
	str	r2,[r9,#CAR_STAT]
	
	;**** mensch
	
	ldr	r2,=car_human
	ldr	r2,[r2]
	cmp	r2,r12
	moveq	r2,#1
	movne	r2,#0
	str	r2,[r9,#CAR_HUMAN]
	
	;****
	
	;mov	r2,lr			;rotationsnummer
	mov	r3,#2			;tilenummer
	mov	r4,#0			;prio
	ldr	r5,=$00008000		;mode
	;mov	r6,lr			;rotation
	;add	lr,lr,#1		;nachste rotnummer
	mov	r6,#0
	
	stmfd	sp!,{lr}
	bl	spr_new_sprite
	
	ldmfd	sp!,{lr}
	
	str	r7,[r9,#CAR_SPRITE_S]
	
	add	r9,r9,#CAR_STRUCT_LEN		;nächste Struct
	
	subs	r12,r12,#1
	bne	get_carsp

	mov	r6,#$4000000

	;**** clr trp
	
	mov	r0,#0
	strh 	r0,[r6,#$50]	
	strh 	r0,[r6,#$52]	
	strh 	r0,[r6,#$54]	

	ldmfd	sp!,{r0-r12,lr}
	bx	lr

;***********************************************************
;*
;* recalc_sprite_positions (berechne sprite views neu) 
;*
;* r0  = R_ROAD
;*
recalc_sprite_anim

	stmfd	sp!,{r0-r12,lr}
	
	
	
	
	mov	r11,r0
	
	ldr	r10,[r11,#R_CARCOUNT]			;anzahl in roadstruct
	ldr	r9,[r11,#R_CARS]			;erster pointer auf cars
	
	ldr	r7,=animphase

for_ca2

	ldr	r5,[r9,#CAR_WINKEL]
	mov	r5,r5, lsl #16
	mov	r5,r5, lsr #16
	
	ldr	r0,[r9,#CAR_SPRITE_S]			;sprite ptr

	;animphase von 0..3

	ldrb	r3,[r9,#CAR_ANIMPHASE_C]
	add	r3,r3,#1
	cmp	r3,#4<<2
	moveq	r3,#0
	strb	r3,[r9,#CAR_ANIMPHASE_C]
	
	ldrb	r4,[r7,r3, lsr #2]	;bekomme animphase
	

	
	;r4 = phase
	mov	r6,r4
	mov	r8,#1		;1 =hflip

	cmp	r5,#4096
	addgt	r6,r4,#6
	movgt	r8,#1
	cmp	r5,#12288
	addgt	r6,r4,#12
	movgt	r8,#0
	cmp	r5,#20480
	addgt	r6,r4,#6
	movgt	r8,#0
	cmp	r5,#28672
	addgt	r6,r4,#0
	movgt	r8,#0
	cmp	r5,#36864
	addgt	r6,r4,#18
	movgt	r8,#0
	cmp	r5,#45056
	addgt	r6,r4,#24
	movgt	r8,#0
	cmp	r5,#53248
	addgt	r6,r4,#18
	movgt	r8,#1
	cmp	r5,#61440
	addgt	r6,r4,#0
	movgt	r8,#1
	
	
	
	;schalte anim auf zweite zeile ab teil 16
	
	cmp	r6,#16
	addge	r6,r6,#64-16
	
	;setze anim phase
	
	bl	spr_getall	;r1 = W1 r2 = W2 r3 = W3
	and	r3,r3,#$fc00	;nur tiles löschen
	orr	r3,r3,r6, lsl #1
	bic	r2,r2,#$3000	;clear h/v
	orr	r2,r2,r8, lsl #12
	bl	spr_setall
	

	add	r9,r9,#CAR_STRUCT_LEN			;nächste struktur

	subs	r10,r10,#1
	bne	for_ca2
	
	ldmfd	sp!,{r0-r12,lr}
	bx	lr

animphase	.byte	0,2,4,2



		.align	4

;***********************************************************
;*
;* recalc_sprite_positions (berechne sprite views neu) 
;*
;* r0  = R_ROAD
;*


	.global	recalc_sprite_positions
	.type	recalc_sprite_positions,function

recalc_sprite_positions

	stmfd	sp!,{r0-r12,lr}
	
	
	mov	r11,r0
	
	ldr	r10,[r11,#R_CARCOUNT]			;anzahl in roadstruct
	ldr	r9,[r11,#R_CARS]			;erster pointer auf cars
	
	ldr	r2,[r11,#R_PLAYFIELD]			;get Playfieldstruktur
	
	ldr	r3,[r2,#PF_X]				;screen x
	ldr	r4,[r2,#PF_Y]				;screen y

for_ca

	ldr	r5,[r9,#CAR_X_POS]
	ldr	r6,[r9,#CAR_Y_POS]
	mov	r5,r5,asr #16
	mov	r6,r6,asr #16
	
	ldr	r0,[r9,#CAR_SPRITE_S]			;sprite ptr
	sub	r1,r5,r3				;posx-screenx
	sub	r2,r6,r4

	bl	spr_change_xy
	

	add	r9,r9,#CAR_STRUCT_LEN			;nächste struktur

	subs	r10,r10,#1
	bne	for_ca
	
	ldmfd	sp!,{r0-r12,lr}
	bx	lr



;***********************************************************
;*
;* set_screen_centered_to_point (setze screen mit mittelpunkt xy) 
;*
;* r0  = R_PLAYFIELD
;* r11 = x
;* r12 = y
;*
	.global	set_screen_centered_to_sprite
	.type	set_screen_centered_to_sprite,function

set_screen_centered_to_sprite

	stmfd	sp!,{r0-r12,lr}

	mov	r11,r11,asr #16
	mov	r12,r12,asr #16
	

	
	mov	r9,r0		;playfield
		
	;r11 = x
	;r12 = y
	
	;**** berechne screenpos
	
	ldr	r0,[r9,#PF_X]		;get Current pos
	ldr	r1,[r9,#PF_Y]
	
	ldr	r2,[r9,#PF_WIDTH]	;screen breite
	ldr	r3,[r9,#PF_HEIGHT]	;
	
	mov	r4,#80			;linke grenze
	mov	r5,#160			;rechte grenze
	
	mov	r6,#53			;obere grenze
	mov	r7,#106			;untere grenze
	
	
	;If((px)>(sx+rx)) sx=px-rx;
	;If((px)<(sx+lx)) sx=px-lx;

	;If((py)>(sy+uy)) sy=py-uy;
	;If((py)<(sy+oy)) sy=py-oy;

	add	r8,r0,r5		;(sx+rx)
	cmp	r11,r8			;px>(sx+rx)
	subgt	r0,r11,r5		;sx=px-rx;
	
	add	r8,r0,r4
	cmp	r11,r8
	sublt	r0,r11,r4
	
	add	r8,r1,r7		;(sx+rx)
	cmp	r12,r8			;px>(sx+rx)
	subgt	r1,r12,r7		;sx=px-rx;
	
	add	r8,r1,r6
	cmp	r12,r8
	sublt	r1,r12,r6	

	;If(sx<0) sx=0
	;If(sy<0) sy=0
	
	cmp	r0,#0
	movle	r0,#0
	
	cmp	r1,#0
	movle	r1,#0

	;If(sx>=gxm-240) sx=gxm-240
	;If(sy>=gym-160) sy=gym-160
	
	sub	r8,r2,#240
	cmp	r0,r8
	movgt	r0,r8	

	sub	r8,r3,#160
	cmp	r1,r8
	movgt	r1,r8

	;mov	r0,#20*16
	;mov	r1,#20*16

	str	r0,[r9,#PF_X]		;get Current pos
	str	r1,[r9,#PF_Y]

	;**** set bg 2
	
	mov	r2,#2			;bg nummer
	ldr	r3,[r9,#PF_BG2_SRC]	;map src
	ldr	r4,[r9,#PF_BG2_DEST]	;map dest
	bl	scroll_bg
	
	;**** set bg 3
	
	ldr	r0,[r9,#PF_X]		;get Current pos
	ldr	r1,[r9,#PF_Y]		;y
	
	mov	r2,#3			;bg nummer
	ldr	r3,[r9,#PF_BG3_SRC]	;map src
	ldr	r4,[r9,#PF_BG3_DEST]	;map dest
	bl	scroll_bg
	
	;**** set bg 1
	
	ldr	r0,[r9,#PF_X]		;get Current pos
	ldr	r1,[r9,#PF_Y]		;y
	
	mov	r2,#1			;bg nummer
	ldr	r3,[r9,#PF_BG1_SRC]	;map src
	ldr	r4,[r9,#PF_BG1_DEST]	;map dest
	bl	scroll_bg
	
	ldmfd	sp!,{r0-r12,lr}
	bx	lr


	
;***********************************************************
;*
;* delete_road (freigeben der resourcen) 
;*
;* r11  = ROAD_STRUCT
;*

	.global	delete_road
	.type	delete_road,function	
	

delete_road

	stmfd	sp!,{r0-r12,lr}

	;**** free playfield

	ldr		r12,[r11,#R_PLAYFIELD]		;get pointer und free struct
	mem_free_mc	membaseW_adr,r12		;free 

	;**** free cars
	
	ldr	r10,[r11,#R_CARS]			;pointer auf cars 
	ldr	r12,[r11,#R_CARCOUNT]			;anzahl
	mov	r9,r10
rem_car

	;**** free alocated sprite

	ldr	r0,[r10,#CAR_SPRITE_S]			;get spritepointer
	bl	spr_rem_sprite

	add	r10,r10,#CAR_STRUCT_LEN			;nächste Struct
	
	subs	r12,r12,#1
	bne	rem_car
	
	
	mem_free_mc	membaseW_adr,r9			;free die ganze struktur
	
	;****

	ldmfd	sp!,{r0-r12,lr}
	bx	lr

;***********************************************************
;*
;* test_drive 
;*
;* r11  = ROAD_STRUCT
;*

	.global	test_drive
	.type	test_drive,function



test_drive

	stmfd	sp!,{r0-r12,lr}


	;**** inti kollision
	
	mov	r6,r11	
	bl	init_car_koll

	;**** init way points	

	ldr	r10,[r11,#R_CARCOUNT]
	ldr	r6,[r11,#R_CARS]		;first car
getps	
	
	;ldr	r2,[r6,#CAR_HUMAN]
	;cmp	r2,#1
	;beq	getpsnext

	mov	r7,r11
	ldr	r0,=search_nextmark_th+1
	mov	lr,pc
	bx	r0

	mov	r2,r0
	mov	r3,r1
	ldr	r0,[r6,#CAR_X_POS]
	ldr	r1,[r6,#CAR_Y_POS]
	
	mov	r4,#$200
	ldr	r5,=init_driveFromTo_th+1
	mov	lr,pc
	bx	r5

getpsnext
	add	r6,r6,#CAR_STRUCT_LEN

	subs	r10,r10,#1
	bne	getps

	;**** main loop
	
mloop

	;mov	r2,#0
	;bl	wait	
	
	;**** init way points	

	ldr	r10,[r11,#R_CARCOUNT]
	ldr	r6,[r11,#R_CARS]		;first car
getps2	
	
	ldr	r2,[r6,#CAR_HUMAN]
	cmp	r2,#1
	beq	getps2next
	
	ldr	r2,[r6,#CAR_XE]
	ldr	r3,[r6,#CAR_YE]
	ldr	r0,[r6,#CAR_X_POS]
	ldr	r1,[r6,#CAR_Y_POS]

	mov	r4,#$200

	ldr	r5,=init_driveFromTo_th+1
	mov	lr,pc
	bx	r5
	
getps2next
	add	r6,r6,#CAR_STRUCT_LEN
	subs	r10,r10,#1
	bne	getps2	
	
	;**** 
	
	mov	r2,#$5000000
	mov	r1,#$0
	strh	r1,[r2]
	
	;**** fahrer steuerung
	
	ldr	r10,[r11,#R_CARCOUNT]
	ldr	r6,[r11,#R_CARS]		;first car
searhum	ldr	r2,[r6,#CAR_HUMAN]
	cmp	r2,#1
	beq	humfd
	add	r6,r6,#CAR_STRUCT_LEN
	subs	r10,r10,#1
	bne	searhum	
	b	humnotfd
humfd	bl	set_human
humnotfd

	;**** step cars
	
	ldr	r10,[r11,#R_CARCOUNT]
	ldr	r6,[r11,#R_CARS]		;first car
stepcars	
		
	mov	r0,r6
	ldr	r5,=step_car_th+1
	mov	lr,pc
	bx	r5				;steo
	
stepcarsnext

	add	r6,r6,#CAR_STRUCT_LEN
	subs	r10,r10,#1
	bne	stepcars

	mov	r2,#$5000000
	mov	r1,#$1f
	strh	r1,[r2]

	;**** teste kollision

	bl	test_car_koll
	
	mov	r2,#$5000000
	mov	r1,#$1f<<5
	strh	r1,[r2]

	;**** wait for vblank
	
	swi	$50000	
	
	;mov	r2,#$0
	;bl	wait
		
	mov	r2,#$5000000
	mov	r1,#$1f
	strh	r1,[r2]

	;**** recalc sprite positionen
	
	mov	r0,r11
	bl	recalc_sprite_positions		;
	mov	r0,r11
	bl	recalc_sprite_anim
	
	mov	r2,#$5000000
	mov	r1,#$1f<<10
	strh	r1,[r2]


	;**** teste ob am endpunkt
	
	ldr	r10,[r11,#R_CARCOUNT]
	ldr	r6,[r11,#R_CARS]		;first car
endpte
	
	ldr	r2,[r6,#CAR_HUMAN]
	cmp	r2,#1
	beq	nichtendpunt
	
	ldr	r5,=test_driveFromTo_th+1
	mov	lr,pc
	bx	r5
	cmp	r0,#0
	beq	nichtendpunt

	mov	r7,r11
	ldr	r0,=search_nextmark_th+1
	mov	lr,pc
	bx	r0

	mov	r2,r0
	mov	r3,r1
	ldr	r0,[r6,#CAR_X_POS]
	ldr	r1,[r6,#CAR_Y_POS]

	mov	r4,#$200

	ldr	r5,=init_driveFromTo_th+1
	mov	lr,pc
	bx	r5
	

nichtendpunt
	add	r6,r6,#CAR_STRUCT_LEN

	subs	r10,r10,#1
	bne	endpte

	;**** setze car prio
	
	ldr	r10,[r11,#R_CARCOUNT]
	ldr	r6,[r11,#R_CARS]		;first car
setcarpr	
	
	mov	r5,r11				;roadstruct
	ldr	r7,=set_carprio_th+1
	mov	lr,pc
	bx	r7
	
	add	r6,r6,#CAR_STRUCT_LEN
	
	subs	r10,r10,#1
	bne	setcarpr

	;**** setze screen
	
	stmfd	sp!,{r11,r12}
	
	ldr	r0,[r11,#R_PLAYFIELD]	;get playfield
	ldr	r1,[r11,#R_CARS]	;get the 1. car
	add	r1,r1,#CAR_STRUCT_LEN*3
	
	ldr	r11,[r1,#CAR_X_POS]	;get cx
	ldr	r12,[r1,#CAR_Y_POS]	;get cy
		
	bl	set_screen_centered_to_sprite	
	
	ldmfd	sp!,{r11,r12}

	b	mloop	


	;**** free kollsision
	
	mov	r6,r11	
	bl	remove_car_koll

	ldmfd	sp!,{r0-r12,lr}
	bx	lr
	
;***********************************************************
;*
;* set_human (ändere bewegung) 
;*
;* r6  = car_struct
;* r11 = road_struct 

set_human

	stmfd	sp!,{r0-r12,lr}

	
	ldr	r5,[r6,#CAR_WINKEL]
	mov	r4,#$4000000
	ldr	r0,[r4,#$130]
	tst	r0,#$10
	addeq	r5,r5,#$300
	tst	r0,#$20
	subeq	r5,r5,#$300
	str	r5,[r6,#CAR_WINKEL]
	

	mov	r0,#$210
	str	r0,[r6,#CAR_SPEED]

	;**** teste pos
	
	ldr	r0,[r6,#CAR_X_POS]
	ldr	r1,[r6,#CAR_Y_POS]
	
	add	r0,r0,#$e0000
	add	r1,r1,#$e0000
	mov	r0,r0,lsr #16+4			;get 16 pos
	mov	r1,r1,lsr #16+4
	
	
	add	r0,r0,r0		; = x*2
	add	r0,r0,r1, lsl #7	; = x + y * 64*2
	
	ldr	r2,[r11,#R_MAP_SP]	;&map
	ldr	r3,[r11,#R_MAP_SPI]	;&map
	
	ldrh	r1,[r2,r0]		;map(x,y)
	and	r8,r1,#$ff00		;koll
	and	r9,r1,#$00ff		;data byte
	ldr	r9,[r3,r9, lsl #2]	;get indexed datas
	
	;**** 
		
	ldr	r0,[r6,#CAR_X_POS]
	ldr	r1,[r6,#CAR_Y_POS]
	ldr	r2,[r6,#CAR_X_POS_O]
	ldr	r3,[r6,#CAR_Y_POS_O]
	str	r0,[r6,#CAR_X_POS_O]
	str	r1,[r6,#CAR_Y_POS_O]
	
	cmp	r0,r2
	cmpeq	r1,r3
	beq	noKoll

	;**** check ob kollision rechtmäßig 
	
	mov	r4,r9
	ldr	r5,[r6,#CAR_BGMODE]
	ldr	r7,=recalc_koll+1
	mov	lr,pc
	bx	r7
	

	;**** kollision jumplist	

	ldr	r7,=_k_jumplist			; xxxx xxxx 0000 0000 
	mov	r8,r8, lsr #6			; 0000 00xx xxxx xx00 
	and	r8,r8,#$3c			; 0000 0000 00xx xx00
	ldr	r7,[r7,r8]			
	mov	lr,pc
	bx	r7
	

	str	r0,[r6,#CAR_X_POS]
	str	r1,[r6,#CAR_Y_POS]


;h	b	h
	
noHVKoll	
noKoll
	ldmfd	sp!,{r0-r12,lr}
	bx	lr

;***********************************************************
;*
;* init_driveFromTo_th (init fahre von zu) 
;*
;* r0 = startx 16:16
;* r1 = starty
;* r2 = endx
;* r3 = endy
;* r4 = speed
;*
;* r6 = car_struct

	.thumb

init_driveFromTo_th

	push	{r4-r7}
		
	str	r0,[r6,#CAR_X_POS]	;startx
	str	r1,[r6,#CAR_Y_POS]	;starty
	str	r2,[r6,#CAR_XE]		;endx
	str	r3,[r6,#CAR_YE]		;endy
	
		
	sub	r5,r2,r0	;dx
	sub	r7,r3,r1	;dy
	
	asr	r0,r5,#16
	asr	r1,r7,#16
	
	mov	r2,#0
	mvn	r2,r2
	lsr	r2,r2,#16	;1bit sign,1bit integral (?),14bit value 
			
	swi	$a		;arctan2 (r0,r1) = r0
	
	str	r0,[r6,#CAR_WINKEL]	;save calculated winkel
	str	r4,[r6,#CAR_SPEED]	;speed = r4 

	str	r5,[r6,#CAR_DX]		;save delta x
	str	r7,[r6,#CAR_DY]		;save delta y

	pop	{r4-r7}
	bx	lr
	
	.arm

	

;***********************************************************
;*
;* test_driveFromTo (teste ob am ziel) 
;*
;* r6 = car_struct
;*
;* return
;*
;* r0 = 0 nicht da / 1 ist da
;*

	.thumb

test_driveFromTo_th

	push	{r1-r5,r7}

	ldr	r2,[r6,#CAR_XE]		;xe
	ldr	r3,[r6,#CAR_YE]		;ye
	ldr	r4,[r6,#CAR_X_POS]		;carxpos = startx
	ldr	r5,[r6,#CAR_Y_POS]		;carypos = starty
	
	ldr	r7,[r6,#CAR_DX]		;get delta x
	ldr	r1,[r6,#CAR_DY]		;get delta y
	
	;r8 = dx (+rechts,  -links) (x2-x1)
	;r9 = dy (+unten,   -oben)
	
	mov	r0,#0
	mvn	r0,r0
	lsr	r0,r0,#16

	;bic	r4,r4,r0	;remove 16bit fract
	;bic	r5,r5,r0	;remove 16bit fract
		
	cmp	r7,#0	;wenn dx+ swap x1,x2
	blt	nxtest
	
	cmp	r2,r4	;x2 mit x1
	blt	weend
	b	nxend	
nxtest
	cmp	r2,r4	;xd,xq
	bgt	weend
nxend	
	
	cmp	r1,#0	;wenn dy+ swap y1,y2
	blt	nytest
	
	cmp	r3,r5	;y2 mit y1
	blt	weend
	b	nyend	
nytest
	cmp	r3,r5	;yd,yq
	bgt	weend
nyend	

	mov	r0,#0			; noch nicht da
	pop	{r1-r5,r7}
	bx	lr
	
weend		
	mov	r0,#1			; ziel erreicht
	pop	{r1-r5,r7}
	bx	lr

	.arm

;***********************************************************
;*
;* set_carprio_th (teste ob am ziel) 
;*
;  r5 = road_struct
;* r6 = car_struct
;*
;* return
;*
;* r0 = 0 nicht da / 1 ist da
;*	

	.thumb

set_carprio_th

	push	{r0,r1,r2,r3,r5,r6,lr}
		
	ldr	r0,[r6,#CAR_X_POS]		;carxpos
	ldr	r1,[r6,#CAR_Y_POS]		;carypos
		
	lsr	r0,r0,#16+4  		;in 16nern
	lsr	r1,r1,#16+4		;

	add	r0,r0,#1
	add	r1,r1,#1

	add	r0,r0,r0		;x*2
	ldr	r2,[r5,#R_MAP_SP]	;
	ldr	r3,[r5,#R_MAP_SPI]	;index
	lsl	r1,r1,#7		;y*128

	add	r2,r2,r0		;+x
	ldrh	r0,[r2,r1]		;+y	
	mov	r1,#$ff
	and	r0,r0,r1		;nur index wichtig
	lsl	r0,r0,#2		;auf long
	ldr	r3,[r3,r0]		;lade
	
	cmp	r3,#$c2			;!!!!! NOCHMAL ANGUCKEN (INDEX FALSCH)
	bne	nc2
	mov	r3,#2
	
nc2	cmp	r3,#$05
	bne	n05
	mov	r3,#1
n05	
	
	;*** setze Sprite bg prio	
	
	ldr	r1,[r6,#CAR_BGMODE]	;old Backroundmode
	
	;**** teste tokens
	
	cmp	r3,#$02			;wenn mapcode $02 dann über allem
	bne	not_over_all
	mov	r1,#0
not_over_all
	
	cmp	r3,#$01			;wenn mapcode $01 dann unter bg1
	bne	not_under_all
	mov	r1,#3
not_under_all	

	;**** 	
		
	str	r1,[r6,#CAR_BGMODE]	;old Backroundmode		
	ldr	r0,[r6,#CAR_SPRITE_S]	;sprite	
	
	ldr	r3,=spr_change_prio	;r0 = spr, r1 = prio
	mov	r2,#3
	mov	lr,pc
	add	lr,r2
	bx	r3	
				
	
	pop	{r0,r1,r2,r3,r5,r6,r7}
	bx	r7	
	
	.arm	


;******************************************
;*
;* step_car_th (führt eine x,y änderung abhängig vom speed aus)
;*
;* r0 = car_struct
;*

	.thumb

step_car_th

;0,2,3,4,6,7,8,  9,10,11,12        - 5 

	push	{r0-r7}

	
	
	ldr	r1,[r0,#CAR_WINKEL]	;r10 = winkel	
	lsl	r1,r1,#16
	lsr	r1,r1,#16		;winkel 0..65535 (in 32ziger schritten)
	
	ldr	r3,[r0,#CAR_X_POS]	;x
	ldr	r4,[r0,#CAR_Y_POS]	;y
	ldr	r2,[r0,#CAR_SPEED]	;v
		
	lsr	r5,r1,#6		;winkel=max 0..1023
	
	ldr	r7,=sinadr		;sin
	lsl	r5,r5,#2
	
	ldr	r6,[r7,r5]		;sin(winkel)
	
	add	r7,#255
	add	r7,#255
	add	r7,#255
	add	r7,#255
	add	r7,#4			;256*4
	
	ldr	r7,[r7,r5]		;cos(winkel)
	
	lsr	r5,r5,#2
	
	mul	r7,r7,r2		;cos(winkel)*v
	mul	r6,r6,r2		;sin(winkel)*v
	
	asr	r7,r7,#8		;8 bit frakt rest
	asr	r6,r6,#8		;8 bit frakt rest
	
	add	r3,r3,r7		;x=x + cos(winkel)*v
	add	r4,r4,r6		;y=y + sin(winkel)*v 
		
	mov	r2,#$80
	lsl	r2,r2,#8		;$8000 = 32768	
	sub	r1,r2,r1		;180-winkel
	lsl	r1,r1,#16
	lsr	r1,r1,#16		;winkel 0..ffff
	
	str	r3,[r0,#CAR_X_POS]	;x
	str	r4,[r0,#CAR_Y_POS]	;y
	str	r1,[r0,#CAR_WINKEL_SPR]	;sprite winkel 0..31
	
	pop	{r0-r7}
	bx	lr
	
	.arm
	
;
car_human	.word	1	

; autoanzahl
car_count	.word	4

; autopositionen x,y
car_startpos	
		
		.word	21*16*65536,2*16*65536
		.word	24*16*65536,5*16*65536
		.word	27*16*65536,2*16*65536
		.word	30*16*65536,5*16*65536
	
		

	.pool


		.align	4
mod		.incbin	"sound/mod/pond.mmod"  
		.align	4

;*** 

			
	.section    	.ewram,"ax"	

car_anim_speed_c	.byte	0

	.end
	