**************************************
****File name:sin_cos.asm

**************************************
    .include "F2407REGS.H"
    .def SIN_COS
    .def THETA_H,THETA_L,SIN_THETA,COS_THETA
    .ref SINTAB_360
    .ref N,D_THETA
THETA_H   .usect "theta",1
THETA_L   .usect "theta",1
SIN_THETA .usect "theta",1
COS_THETA .usect "theta",1
NXT_ENTRY .usect "theta",1

SIN_COS:
    NOP
**************************************
**** THETA=N*D_THETA
**************************************
    LDP   #N
    ;SPM   1
    LACL  N
	ADD   #1
	SACL  N
    LT    D_THETA
    MPY   N             ;Q13*Q0
    PAC                 ;Q14
    SACH  THETA_H,2 	;Table pointer
	AND   #3FFFH        ;Force THETA_L to a positive number
	SACL  THETA_L,1		;Q15
	LACL  THETA_H
	AND   #0FFH
	SACL  THETA_H
	
******************************************
****Calculate the sin_theta

******************************************
	LACC  THETA_H
	ADD	  #SINTAB_360
	TBLR  SIN_THETA		;sin_theta = Sin(theta) in Q15
	ADD	  #1H			;Inc Table pointer
	TBLR  NXT_ENTRY		;Get next entry i.e. (Entry + 1)
	LACC  NXT_ENTRY
	SUB	  SIN_THETA		;Find Delta of 2 points
	SACL  NXT_ENTRY
	LT	  NXT_ENTRY
	MPY	  THETA_L		;THETA_L = interpolation value
	LACC  SIN_THETA,16
	APAC
	SACH  SIN_THETA		;sin_theta = Final interpolated value   
*******************************************
****Calculate the cos_theta

*******************************************
    LACL  THETA_H
    ADD   #40H          ;COS_THETA=SIN_THETA+90,256/4=64->40H
    AND   #0FFH
    ADD   #SINTAB_360
    TBLR  COS_THETA
    ADD	  #1H			;Inc Table pointer
	TBLR  NXT_ENTRY		;Get next entry i.e. (Entry + 1)
	LACC  NXT_ENTRY
	SUB	  COS_THETA		;Find Delta of 2 points
	SACL  NXT_ENTRY
	LT	  NXT_ENTRY
	MPY	  THETA_L		;THETA_L = interpolation value
	LACC  COS_THETA,16
	APAC
	SACH  COS_THETA		;sin_theta = Final interpolated value 
    RET