;Filename:fft_func.inc
		.text
bit_rev:
		NOP
		RSBX	CMPT
		SSBX	FRCT				; Fractional mode is on
		SSBX	SXM
		LD		#-1,ASM
		STM		#d_input_addr,AR3
		STM		#d_input_circ,AR2
		STM		#K_FFT_SIZE-1,BRC
		RPTB	data_move-1
		MVDD	*AR2+,*AR3+
		ST	   	#0h,*AR3+			; Real input data 256--> complex data 512
data_move:
		STM		#d_input_addr,AR3	; AR3 --> 1 st original input
		STM 	#fft_data,AR7 		; AR7 --> data processing buffer
		MVMM 	AR7,AR2 			; AR2 --> 1st bit-reversed data
		STM 	#K_FFT_SIZE-1,BRC
		RPTBD 	bit_rev_end-1
		STM 	#K_FFT_SIZE,AR0 	; AR0 = 1/2 size of circ buffer
		MVDD 	*AR3+,*AR2+
		MVDD 	*AR3-,*AR2+
		MAR 	*AR3+0B
bit_rev_end:
		NOP
		RET 						;AR7 is reserved for fft_data
		
;FFT algorithm
shift1bit:
		NOP
		;SSBX	SXM
		STM		#fft_data,AR2
		STM		#511,BRC
		RPTB	shft1b-1
		LD		*AR2,A
		SFTA	A,#-1
		STL		A,*AR2+
shft1b
		NOP
		RET


fft_test:
		
		STM		#fft_data,AR2
		STM		#fft_data,AR3
		MAR		*AR3+
		STM		#fft_data_tst,AR4
		STM		#255,BRC
		RPTB	tst_fft-1
		SQUR	*AR2+,A
		SQUR	*AR3+,B
		ADD		B,A	
		MAR		*AR2+
		MAR		*AR3+
		;STH		A,ASM,*AR4+
		STH		A,*AR4+
tst_fft		
		RET
;Complex FFT 256-point
fft_task:		;AR7-->first address of fft_data
;Stage 1
		STM		#fft_data,AR2	;AR2-->x(I)
		STM		#fft_data+2,AR4	;AR4-->x(IP)
		STM		#2,AR0
		STM     #127,BRC
		RPTB	stg1end-1      
		SUB		*AR2,*AR4,A
		ADD		*AR2,*AR4,B
		STH		A,ASM,*AR4+
		STH		B,ASM,*AR2+
		SUB		*AR2,*AR4,A
		ADD		*AR2,*AR4,B
		STH		A,ASM,*AR4+
		STH		B,ASM,*AR2+
		MAR		*AR2+0
		MAR		*AR4+0
stg1end
;Stage 2
		STM		#fft_data,AR2	;AR2-->x(I)
		STM		#fft_data+4,AR4	;AR4-->x(IP)
		STM		#6,AR0			;AR2 <-->AR4  
		STM     #63,BRC
		RPTB	stg2half-1      
		SUB		*AR2,*AR4,A
		ADD		*AR2,*AR4,B
		STH		A,ASM,*AR4+
		STH		B,ASM,*AR2+
		SUB		*AR2,*AR4,A
		ADD		*AR2,*AR4,B
		STH		A,ASM,*AR4+
		STH		B,ASM,*AR2+
		MAR		*AR2+0
		MAR		*AR4+0
stg2half
		STM		#T_v,AR3
		STM		#fft_data+2,AR2	;AR2-->x(I)
		STM		#fft_data+6,AR4
		STM		#6,AR0			;AR4-->x(IP) 
		STM     #63,BRC
		RPTB	stg2end-1      
		MAR		*AR4+
		SUB		*AR2+,*AR4-,A
		ADD		*AR2-,*AR4+,B
		STH		A,ASM,*AR3+
		STH		B,ASM,*AR3-
		ADD		*AR2+,*AR4-,A
		SUB		*AR2-,*AR4,B
		STH		A,ASM,*AR2+
		STH		B,ASM,*AR2+
		MVDD	*AR3+,*AR4+
		MVDD	*AR3-,*AR4+
		;NOP
		MAR		*AR2+0
		MAR		*AR4+0
stg2end
;Stage 3 to logN
		STM		#L_cnt,AR1
		ST		#2h,*AR1			;Used AR1  -->L_cnt
		STM		#LE_v+2,AR5		;	  AR5  -->LE_v
		STM		#LE1_v+2,AR6		;	  AR6  -->LE1_v
		STM		#U_v,AR2		;	  AR2  -->U_v
 		STM		#weight_fact+4,AR3;	  AR3  -->weight_fact
lev_jmp:						
		;NOP
		;==========
		ST		#7FFFh,*AR2+	;U1=1.0
		ST		#0h,*AR2-		;U2=0.0
		;MAR		*AR2-
		PSHM	AR1
		STM		#J_v,AR1
		ST      #0h,*AR1		;J=0
butfly:
		;==========
		;NOP						;Add here
		STM		#I_v,AR0
		;MVDD   	*AR1,*AR0  	;exchange *AR1 and *AR0, but only AR2,3,4,5
		LD		*AR1,A
		STL		A,*AR0			;I=J=0
		;NOP
innerfly
		;NOP
		LD		*AR0,A
		ADD		*AR6,A			;IP=I+LE1
		SFTL	A,#1			;IP_cnt=2*IP
		LDM		AR7,B
		ADD		B,A
		STLM	A,AR4			;AR4-->x(IP_cnt)
		;NOP
		;NOP
		LD		#0h,A			;AR2-->U_v
		LD		#0h,B
		MAC		*AR2+,*AR4+,A
		MAS		*AR2-,*AR4,A
		MAC 	*AR2+,*AR4-,B
		MAC		*AR2-,*AR4,B	;AR2-->U_v
		;NOP
		
		PSHM	AR2	
		PSHM	AR3		
		STM		#T_v,AR2	;AR2-->T_v		
		STH		A,ASM,*AR2+
		STH		B,ASM,*AR2-
		;MAR		*AR2-		;AR2-->T_v

		LD		*AR0,A
		SFTL	A,#1		;2I=2*I
		NOP
		LDM	    AR7,B				
		ADD     B,A	
		STLM    A,AR3		;AR3-->x(2I)
		NOP					;must have
		NOP					;must have
		;NOP
		
		LD		*AR3,A
		SFTA	A,#-1
		STL		A,*AR3+
		LD		*AR3,A
		SFTA	A,#-1
		STL		A,*AR3-		;AR3-->x(2I)/2
		
		SUB		*AR3+,*AR2+,A
		STH	    A,*AR4+		;
		;MAR	*AR3+
		;MAR	*AR2+
		SUB		*AR3-,*AR2-,A
		STH     A,*AR4-		;x(IP_cnt) finished
		;MAR    *AR4-
		;MAR	*AR2-
		;MAR	*AR3-
		
		ADD		*AR3+,*AR2+,A
		ADD 	*AR3-,*AR2-,B
		;MAR    *AR3-
		STH     A,*AR3+
		STH	    B,*AR3-     ;x(I) finished
		;NOP
		
		POPM	AR3
		POPM	AR2			;AR2-->U_v
		
		LD		*AR0,A
		ADD		*AR5,A
		STL		A,*AR0
		SUB		#256,A	
		BC   	innerfly,ALT
		;==========			;Add above
		;U_v=U_v*W
		LD		#0h,A
		LD		#0h,B
		MAC		*AR2+,*AR3+,A
		MAS		*AR2-,*AR3,A
		MAC 	*AR2+,*AR3-,B
		MAC		*AR2-,*AR3,B
		STH		A,*AR2+
		STH		B,*AR2-		;AR3-->W
							;AR2-->U_v
		
		;==========
		ADDM	#1,*AR1
		LD		*AR6,B
		SUB		*AR1,B
		BC		butfly,BGT
		
		POPM	AR1	
		MAR		*AR6+
		MAR		*AR3+
		MAR		*AR3+
		MAR		*AR5+		
		;===========
		ADDM	#1,*AR1
		BITF	*AR1,#0008h
		BC		lev_jmp,NTC
		RET