.include "ccall.asm"
M .set 9 ; Sine table size log2
                  
	.ref _SineTable                  

	.def	_ti_cos
_ti_cos:
	pre_ccall 3,AR_X,AR_Y,AR_N
;STACK CONTENTS:
; 1 = sin_Index / sin_Ynext / sin_Ydiff
; 2 = sin_Frac
; 3 = 
	; Modify any registers needed
	spm	1		; MUST Set Product shift to +1
	clrc	SXM		; MUST Set Sign Extension Mode Off
	; Note: Overflow mode is off in C.
; End C Preprocessing

; Subtract one from N and check for N=0
	mar *,AR_N
	banz COSLOOP,AR_X
	b COSDONE

;	ldp	#sin_X		
;	lacc	sin_X,8		; Index = integer(X + Phase)
COSLOOP:
	lacc *+,M-1,AR_STACK
 .if (M==9)
	add #128,15 ; Add 2^(M-2)
 .endif	                  
 	
;;;	add	sin_Phase,8	;;; UNUSED

;	sach sin_Index	
;   sacl sin_Frac
	sach *+,1
	and #07fffh ; Zero sign bit
	sacl *-

;	lacc	sin_Index	; Y = SinTable[Index], 
	lacc *,AR_Y
 .if (M==9)
	and #511 ; And (2^M)-1 to keep inside table
 .endif
	add #_SineTable	
;	tblr sin_Y
	tblr *,AR_STACK
	add	#1
;	tblr sin_Ynext	; Ynext = SinTable[Index+1],
	tblr *

;	lacc sin_Ynext	; Ydiff = Ynext - Y
	lacc *,AR_Y
;	sub	sin_Y
	sub *,AR_STACK
;	sacl sin_Ydiff
	sacl *

;	lt sin_Ydiff	; Y = round(Y + Ydiff*Frac)
	lt *+
;	mpy	sin_Frac
	mpy *-,AR_Y
;	zalr sin_Y
	zalr *
	apac
;	sach sin_Y	
	sach *+,AR_N
	banz COSLOOP,AR_X

; Begin C Post Processing
COSDONE:
	spm	0
	post_ccall 3