NAME	SAMPLE

?PR?_sample?SAMPLE   SEGMENT CODE 
?PR?phase?SAMPLE    SEGMENT CODE
?PR?_sum?SAMPLE    SEGMENT CODE
?PR?_exceeds?SAMPLE    SEGMENT CODE

	PUBLIC	_sample
	PUBLIC	phase
	PUBLIC	_sum
	PUBLIC	_exceeds

; void sample( unsigned short timerValue )

	RSEG  ?PR?_sample?SAMPLE
_sample:
	USING	0
			; SOURCE LINE # 2
;---- Variable 'timerValue?040' assigned to Register 'R6/R7' ----
; {
			; SOURCE LINE # 3
; 	timerValue+=1;
			; SOURCE LINE # 4
;	INC  	R7
;	CJNE 	R7,#00H,?C0002
;	INC  	R6
;?C0002:
; 	#pragma asm
; 	Note R7 is low byte of timer value
;   R6 is high byte
;   stop timer

;    reset dptr 
mov dptr, #0

push 0E0h
push 0
push 1
push 2
push 3
push 4
push 082h
push 083h

// stop timer0
clr 088h.4
;   we need to set timer t0 to this value mod 4
;   as this is the most accurate we can test the timer
; 	reset TCON.5 ( interrupt bit )  

;;;;;;; pre setup for sampling
clr A // 
mov r0, #255  // 1024 byte count down
mov r1, #4

clr 088h.5
mov 08Ah, R7 ; TL0
mov 08Ch, R6 ; TH0

;   send 8 cycle pulse 
mov r2, #16

; set ultrasonic do-dad to initial state
;clr 080h.0 
;setb 080h.1
mov A, R5
anl A, #5 ; 0101
mov 080h, A

pulse:

; pulse timed for 40khz frequency
mov r4, #096 ; 47
djnz r4, $

nop
nop

; toggle ultrasonic do-dad
;cpl 080h.0
;cpl 080h.1
mov A, R5
xrl 080h,A

djnz r2, pulse

; set ultrasonic do-dad output back to gnd
mov 080h, #0

;   start timer
setb 088h.4  ; 2 cycles

;   while no timer overflow loop to 0
jnb 088h.5, $ ; 4 cycles per check, 3 on final

; record timer value
; MOV  	R7, 08Ah
; MOV  	R6, 08Ch

; 		/*
; 		note: 60 samples per wave = 0.145mm accuracy
; 		*/
; 		// read almost starting at 1k worth of inputs
; max sample rate is 100 per wave, set to 60 per wave atm
readLoop:
			mov A, 090h ;P1 2
			swap A      ;   1
			nop
			nop
			nop
			nop
			nop
			nop
			nop         ;   7
						
			orl A, 090h ;   2
			movx @DPTR, A ; 3
			inc DPTR ;      1
			nop           ; 1
			djnz r0, readLoop ; 3

			dec r0

			mov A, 090h ;P1 2
			swap A      ;   1
			nop
			nop
			nop
			nop
			nop
			nop
			nop         ;   7
						
			orl A, 090h ;   2
			movx @DPTR, A ; 3
			inc DPTR ;      1
			nop           ; 1

			djnz r1, readLoop  ; 3
			

; 		/*
; 		*/
; 		// filter and evaluate phase values
; 		// find mid-points
; 		// update position using position prediction value cutoffs
; 		// update position predictions
; 		// store position in usb io buffer
; 		// wait appropriate time for echos to die down.

;   stop timer
clr 0x88.5

pop 083h
pop 082h
pop 4
pop 3
pop 2
pop 1
pop 0
pop 0E0h

; 	return 1234;
	; SOURCE LINE # 9
;	MOV  	R6,#04H
;	MOV  	R7,#0D2H
; }			; SOURCE LINE # 10
?C0001:
; 	#pragma endasm
; }			; SOURCE LINE # 8
	RET  	
; END OF _sample

; 

; unsigned short phase( unsigned short timerStart, unsigned short timerLength )

	RSEG  ?PR?phase?SAMPLE
phase:
	USING	0
			; SOURCE LINE # 12
;---- Variable 'timerLength?142' assigned to Register 'R4/R5' ----
;---- Variable 'timerStart?141' assigned to Register 'R6/R7' ----
; {
			; SOURCE LINE # 13
; 	timerStart+=1;
			; SOURCE LINE # 14
			; R7 timerStart low, R6 timerStart high 
?C0004:
; 	timerLength+=1;
			; SOURCE LINE # 15
?C0005:

push 0E0h
push 0
push 1
push 2
push 3
push 4
push 5
push 6
push 7
push 082h
push 083h

; ****** FIRST RECIEVER ********
	; clear eval memory
	mov R0, #134
	mov R3, #120 ; 60 * 2
	clr A
loop_1_clear:
	mov @R0, A
	inc R0
	djnz R3, loop_1_clear;


	; SUM all channel 1 values into phase table (size 60)
	mov DPTR, #0
	mov R4, #17 ; 1020 samples

loop_1_outer:
	; 30h in steps of 2 has 0->59 sums
	mov R0, #134
	; mod value
	mov R3, #30  ; mod 60 2 samples at a time

loop_1:
	movx A, @DPTR ;  A = @DPTR
	mov R5, A     ;  R5 = A
	swap A        ;  A <-> A
	anl A, #1     ;  A &= 1

	add A, @R0    ;  A += @R0 (low byte)
	mov @R0, A    ;  @R0 = A (save low byte)
	inc R0        ;  move to high byte
	mov A, @R0    ;  A = @R0 (high byte)
	addc A, #0    ;  A += Carry
	mov @R0, A    ;  @R0 = A
	inc R0 ; move to next sum value

	mov A, R5
	anl A, #1

	add A, @R0    ;  A += @R0 (low byte)
	mov @R0, A    ;  @R0 = A (save low byte)
	inc R0        ;  move to high byte
	mov A, @R0    ;  A = @R0 (high byte)
	addc A, #0    ;  A += Carry
	mov @R0, A    ;  @R0 = A
	inc R0 ; move to next sum value

	; move to next data value
	inc DPTR

	; make sure R0 is mod 60 / ( 94 )
	djnz R3, loop_1
	djnz R4, loop_1_outer

	acall find_mid_point;

	mov DPTR, #1021
	mov A, R0
	movx @DPTR, A

; ****** SECOND RECIEVER ********
	; clear eval memory
	mov R0, #134
	mov R3, #120 ; 60 * 2
	clr A
loop_2_clear:
	mov @R0, A
	inc R0
	djnz R3, loop_2_clear;


	; SUM all channel 1 values into phase table (size 60)
	mov DPTR, #0
	mov R4, #17 ; 1020 samples

loop_2_outer:
	; 30h in steps of 2 has 0->49 sums
	mov R0, #134
	; mod value
	mov R3, #30

loop_2:
	movx A, @DPTR ;  A = @DPTR
	rr A
	mov R5, A     ;  R5 = A
	swap A        ;  A <-> A
	anl A, #1     ;  A &= 1

	add A, @R0    ;  A += @R0 (low byte)
	mov @R0, A    ;  @R0 = A (save low byte)
	inc R0        ;  move to high byte
	mov A, @R0    ;  A = @R0 (high byte)
	addc A, #0    ;  A += Carry
	mov @R0, A    ;  @R0 = A
	inc R0 ; move to next sum value

	mov A, R5
	anl A, #1

	add A, @R0    ;  A += @R0 (low byte)
	mov @R0, A    ;  @R0 = A (save low byte)
	inc R0        ;  move to high byte
	mov A, @R0    ;  A = @R0 (high byte)
	addc A, #0    ;  A += Carry
	mov @R0, A    ;  @R0 = A
	inc R0 ; move to next sum value

	; move to next data value
	inc DPTR

	; make sure R0 is mod 60 / ( 94 )
	djnz R3, loop_2
	djnz R4, loop_2_outer

	acall find_mid_point;

	mov DPTR, #1022
	mov A, R0
	movx @DPTR, A

; ****** THREE RECIEVER ********
	; clear eval memory
	mov R0, #134
	mov R3, #120 ; 60 * 2
	clr A
loop_3_clear:
	mov @R0, A
	inc R0
	djnz R3, loop_3_clear;


	; SUM all channel 1 values into phase table (size 60)
	mov DPTR, #0
	mov R4, #17 ; 1020 samples

loop_3_outer:
	; 30h in steps of 2 has 0->49 sums
	mov R0, #134
	; mod value
	mov R3, #30

loop_3:
	movx A, @DPTR ;  A = @DPTR
	rr A
	rr A
	mov R5, A     ;  R5 = A
	swap A        ;  A <-> A
	anl A, #1     ;  A &= 1

	add A, @R0    ;  A += @R0 (low byte)
	mov @R0, A    ;  @R0 = A (save low byte)
	inc R0        ;  move to high byte
	mov A, @R0    ;  A = @R0 (high byte)
	addc A, #0    ;  A += Carry
	mov @R0, A    ;  @R0 = A
	inc R0 ; move to next sum value

	mov A, R5
	anl A, #1

	add A, @R0    ;  A += @R0 (low byte)
	mov @R0, A    ;  @R0 = A (save low byte)
	inc R0        ;  move to high byte
	mov A, @R0    ;  A = @R0 (high byte)
	addc A, #0    ;  A += Carry
	mov @R0, A    ;  @R0 = A
	inc R0 ; move to next sum value

	; move to next data value
	inc DPTR

	; make sure R0 is mod 60 / ( 94 )
	djnz R3, loop_3
	djnz R4, loop_3_outer

	acall find_mid_point;

	mov DPTR, #1023
	mov A, R0
	movx @DPTR, A

pop 083h
pop 082h
pop 7
pop 6
pop 5
pop 4
pop 3
pop 2
pop 1
pop 0
pop 0E0h

	RET  
	
find_mid_point:
	clr A
	;	byte i;
	; R1 = i
	;	short diff=0;
	mov R2, A ; low
	mov R3, A ; high
	;	for( i=0; i<30; i++ )
	mov R7, #30
	// mov R1, #0
	mov R1, #134
loop_mp1:
	; matchCount[i]
	;diff -= matchCount[i];
	clr C ; Always clear carry before first subtraction
	mov A, R2 ; Move the low-byte into the accumulator
	subb A, @R1 ; Subtract the second low-byte from the accumulator
	mov R2, A ; Move the answer to the low-byte of the result
	inc R1

  mov A,R3  ;Move the high-byte into the accumulator
  subb A,@R1 ;Subtract the second high-byte from the accumulator
  mov R3,A  ;Move the answer to the low-byte of the result
	inc R1

	djnz R7, loop_mp1
	
	;	for( i=30; i<60; i++ ) 
	mov R7, #30
loop_mp2:
	; diff += matchCount[i];
	mov A, @R1 ; Move the low-byte into the accumulator
	inc R1
	add A, R2 ; Subtract the second low-byte from the accumulator
	mov R2, A ; Move the answer to the low-byte of the result

  mov A, @R1  ;Move the high-byte into the accumulator
  inc R1
	addc A,R3 ;Subtract the second high-byte from the accumulator
  mov R3,A  ;Move the answer to the low-byte of the result

	djnz R7, loop_mp2


	; ***************************************************************************
	;	maxi=0
	clr A
	mov R4, A
	; short maxp=16384;
	mov R5, A
	mov R6, #64

	; byte j=30;
	mov R0, #134+30*2

	; for (i = 0; i < 60; i++)
	mov R1, #134
loop_max_diff:
	; {
	; if ( diff>=0
	mov A, R3
	rlc A
	jc skip_no_greater

	; && maxp(R5,R6) < diff(R2,R3) )
	clr c
	mov A, R6
	subb A, R3
	jc greater

; otherwise high bytes are equal	

	mov A, R5
	subb A, R2
	jnc skip_no_greater

greater:

	; {
	; maxi = i;
	mov A, R1
	mov R4, A
	; maxp = diff;
	mov A, R2
	mov R5, A
	mov A, R3
	mov R6, A
	; }
skip_no_greater:

	; diff += matchCount[i];
	mov A, @R0  ; a = low byte
	inc R0      ; move on

	add A, R2   ; diff low += a low
	mov R2, A   ; 
	mov A, @R0  ; a = high byte
	dec R0	

	addc A, R3  ; diff high += a high
	mov R3, A   ; 

	; diff -= matchCount[j];
	clr c       ; 
	mov A, R2   ; 
	subb A, @R1  ; diff low -= a low
	mov R2, A   ;
	inc R1      ; move on

	mov A, R3   ;
	subb A, @R1  ;
	mov R3, A   ; diff low -= a low
	dec R1

	; *2 make this nicer
	mov A, @R0  ; a = low byte
	inc R0      ; move on

	add A, R2   ; diff low += a low
	mov R2, A   ; 
	mov A, @R0  ; a = high byte
	inc R0	

	addc A, R3  ; diff high += a high
	mov R3, A   ; 

	; diff -= matchCount[j];
	clr c       ; 
	mov A, R2   ; 
	subb A, @R1  ; diff low -= a low
	mov R2, A   ;
	inc R1      ; move on

	mov A, R3   ;
	subb A, @R1  ;
	mov R3, A   ; diff low -= a low
	inc R1


; **************************************************

	; j++;

	; if (j == 60)
	cjne R0, #134+60*2, no_mod_overflow
	; {
	; j = 0;
	mov R0, #134
	; }
no_mod_overflow:
	; }
	cjne R1, #134+60*2, loop_max_diff

	; R4 is offset
	; get index rather than pointer
	clr c
	mov A, R4
	subb A, #134
	rrc A
	mov R4, A
	; ========================================================

	; R4 = index

	; if( matchCount[maxi]<matchCount[(maxi+30)%60] )
	; R1 = (maxi+30)%60
	; mov A, R0
	clr c
	subb A, #29
	mov A, R4
	jnc not_greater_maxi2
	
	add A, #60
not_greater_maxi2:
	subb A, #30	

	mov R2, A ; r2 = j, R4 = i

	; r0 = i
	; r0 = memloc i	
	clr c
	mov A, R4 ; shorts
	rlc A
	add A, #134
	mov R0, A

	; r1 = memloc j
	clr c
	mov A, R2 ; shorts
	rlc A
	add A, #134
	mov R1, A
	
	// cmp
	clr c
	mov A, @R1
	subb A, @R0   ;;; A = [j] - [i]
	jc not_greater_max1
	inc R0
	inc R1
	mov A, @R1
	subb A, @R0
	jc not_greater_max1  // if [j] > [i]
	

	; { maxi+=30
	mov A, R2
	mov R4, A
	; }
not_greater_max1:	

	mov A, R4
	mov R0, A

	; ========================================================



	RET
		
; END OF _phase
	

	RSEG  ?PR?_sum?SAMPLE
_sum:
	USING	0
	push 0E0h 
	push 0
	push 1
	push 2
	push 3
	push 082h
	push 083h

	; 2,3 sum
	clr A
	mov R2,A
	mov R3,A

	; 4,5 start
	mov 083h, R6 ; DPH
	mov 082h, R7 ; DPL

	; 6,7 end
	mov R1, #1	
	mov R6, #0

sumLoop:
	movx A, @DPTR	
	mov R0, A // temp R2
	inc DPTR // advance through memory

	anl A, R1 // bit 1
	add A, R3
	mov R3, A
	mov A, R2
	addc A, R6
	mov R2, A

	mov A, R0
	rr A
	mov R0, A
	anl A, R1 // bit 2
	add A, R3
	mov R3, A	
	mov A, R2
	addc A, R6
	mov R2, A

	mov A, R0
	rr A
	mov R0, A
	anl A, R1 // bit 3
	add A, R3
	mov R3, A
	mov A, R2
	addc A, R6
	mov R2, A

	mov A, R0
	rr A
	rr A
	mov R0, A
	anl A, R1 // bit 1
	add A, R3
	mov R3, A
	mov A, R2
	addc A, R6
	mov R2, A

	mov A, R0
	rr A
	mov R0, A
	anl A, R1 // bit 2
	add A, R3
	mov R3, A
	mov A, R2
	addc A, R6
	mov R2, A

	mov A, R0
	rr A
	anl A, R1 // bit 3
	add A, R3
	mov R3, A
	mov A, R2
	addc A, R6
	mov R2, A

	mov A, R4
	cjne A, 083h, sumLoop
	mov A, R5
	cjne A, 082h, sumLoop

	mov A,R2
	mov R6,A
	mov A,R3
	mov R7,A

	pop 083h
	pop 082h
	pop 3
	pop 2
	pop 1
	pop 0
	pop 0E0h

	RET

	RSEG  ?PR?_exceeds?SAMPLE
_exceeds:
	USING	0
	push 0E0h
	push 0
	push 1
	push 2
	push 3
	push 4
	push 5
	push 082h
	push 083h

	mov DPTR, #0
	// 082h DPL
	// 083h DPH

	mov R3,#1
	clr c

exceedsLoop:
	mov A, R5
	mov R6, A

	movx A, @DPTR
	inc DPTR
	
	// shift left by which (R6)
	rlc A
exceedsLoopA:
	rrc A
	djnz R6, exceedsLoopA

  // R2 = temp value
	mov R2, A
	
	swap A
	anl A, R3
	mov R0, A

	mov A, R2
	anl A, R3
	add A, R0

	mov R4, A
	mov A, R7
	subb A, R4
	mov R7, A

	jc skipExceedsLoop
	mov A, #4 ; 1024 end
	cjne A, 083h, exceedsLoop
	
skipExceedsLoop:
	clr c

	mov A, 082h
	rlc A
	mov R7, A

	mov A, 083h
	rlc A
	mov R6, A


	pop 083h
	pop 082h
	pop 5
	pop 4
	pop 3
	pop 2
	pop 1
	pop 0
	pop 0E0h
	RET

	END