cblock
	ARGh,ARGl,ACCh,ACCl,PROD3,PROD2,PROD1,PROD0,Temp3,Temp2,Temp1,Temp0
	ANS1,ANS0,DIV1,DIV0,DivCnt
	endc
;Mathematical Operations
;16-bit by 16-bit addition
Add16x16				;(ACCh ACCl)=(ACCh ACCl)+(ARGh ARGl)
	movf	ARGl,W		;W=ARGl
	addwf	ACCl,F		;ACCl=ACCl+ARGl
	btfsc	STATUS,C
	incf	ACCh,F		;if(carry==1)	ACCh++
	movf	ARGh,W		;W=ARGh
	addwf	ACCh,F		;ACCh=ACCh+ARGh
	return

;16-bit by 16-bit subtraction
Sub16x16				;(ACCh ACCl)=(ACCh ACCl)-(ARGh ARGl)
	comf	ARGl,F
	incf	ARGl,F		;ARGl=-ARGl
	btfsc	STATUS,Z
	decf	ARGh,F		;if(ARGl==0)	ARGh--
	comf	ARGh,F

	movf	ARGl,W		;W=ARGl
	addwf	ACCl,F		;ACCl=ACCl+ARGl
	btfsc	STATUS,C
	incf	ACCh,F		;if(carry==1)	ACCh++
	movf	ARGh,W		;W=ARGh
	addwf	ACCh,F		;ACCh=ACCh+ARGh
	return

;16-bit by 16-bit multiplication
Mul16x16			;(PROD3 PROD2 PROD1 PROD0)=(ACCh ACCl)*(ARGh ARGl)
	clrf4	Temp3,Temp2,Temp1,Temp0
	clrf4	PROD3,PROD2,PROD1,PROD0
	mov2	Temp0,Temp1, ACCl,ACCh
	movf	ACCh,F
	jnz		CheckNext	;if((ACCh ACCl)!=0)	goto	CheckNext
	movf	ACCl,F
	jz		Equal0		;if((ACCh ACCl)==0)	goto	Equal0
CheckNext
	movf	ARGh,F
	jnz		DoMultiply	;if(ARGh!=0)	goto	DoMultiply
	movf	ARGl,F
	jz		Equal0		;if(ARGl==0)	goto	Equal0
DoMultiply
	movf	ARGh,F
	jnz		TestLSB		;if(ARGh!=0)	goto	TestLSB
	movf	ARGl,F
	btfsc	STATUS,Z	;if(ARGl!=0)	goto	TestLSB
	return				;if((ARGh ARGl)==0)	return
TestLSB
	rrf2	ARGh,ARGl
	jnc		DoShift		;if(carry==0)	goto	DoShift
	movf	Temp0,W		;
	addwf	PROD0,F  	;PROD0+=Temp0
	jnc		ADD2
	movlw	1			;Add carry bit if necessary
	addwf	PROD1,F
	jnc		ADD2
	movlw	1
	addwf	PROD2,F
	jnc		ADD2
	incf	PROD3,F
ADD2
	movf	Temp1,W
	addwf	PROD1,F		;PROD1+=Temp1
	jnc		ADD3
	movlw	1
	addwf	PROD2,F
	jnc		ADD3
	incf	PROD3,F
ADD3
	movf	Temp2,W
	addwf	PROD2,F		;PROD2+=Temp2
	jnc		ADD4
	incf	PROD3,F
ADD4
	movf	Temp3,W
	addwf	PROD3,F		;PROD3+=Temp3
DoShift
	rlf4	Temp3,Temp2,Temp1,Temp0
	goto	DoMultiply
Equal0
	clrf4	PROD3,PROD2,PROD1,PROD0
	return

;32-bit x 16-bit division
Div32x16				;(ANS1 ANS0)=(PROD3 PROD2 PROD1 PROD0)/(DIV1 DIV0)
	clrf2	ANS1,ANS0	;Clear the result registers
	mvi		DivCnt,17
DA1
	movf	DIV1,W
	subwf	PROD3,W		;W=PROD3-DIV1
	jnc		NoSub		;if(DIV1>PROD3)	goto	NoSub
	jnz		DoSubs		;if(DIV1<PROD3)	goto	DoSubs
	movf	DIV0,W		;if(DIV1==PROD3)	then
	subwf	PROD2,W		;W=PROD2-DIV0
	jc		DoSubs		;if(DIV0<PROD2)	goto	DoSubs
NoSub
	rlf2	ANS1,ANS0	;Add 0 to LSB of (ANS1 ANS0)
	rlf4	PROD3,PROD2,PROD1,PROD0
	goto	ChkCnt
DoSubs
	mov2	ACCh,ACCl, PROD3,PROD2
	mov2	ARGh,ARGl, DIV1,DIV0
	call	Sub16x16
	mov2	PROD3,PROD2, ACCh,ACCl	;(PROD3 PROD2)=(PROD3 PROD2)-(DIV1 DIV0)
	bsf		STATUS,C
	rlf		ANS0,F
	rlf		ANS1,F					;Add 1 to LSB of (ANS1 ANS0)
	rlf4	PROD3,PROD2,PROD1,PROD0	;Shift(PROD3 PROD2 PROD1 PROD0)left
ChkCnt
	djnz	DivCnt,DA1				;Check for 17 operations
	return