;============================================================================
; PROJECT     : PIC'SPECTRUM
;
; FILE        : FFT.INC
;
; VERSION     : 1.0
;
; DESCRIPTION :
;
; 256 points FFT routines, using s/2/13 format for numeric computations
; (through the fixed.inc library). All data are stored in the internal
; registers of the 17C756 (array DATA).
;
; Execution time on a PIC17C756/32MHz : around 50ms without concurrent
; video generation (resulting in a 90-100ms refresh time with spectrum
; power calculation, analog acquisition and video refresh)
;
;============================================================================
;                Developped & Copyrighted by Robert LACOSTE
;============================================================================



;----------------------------------------------------------------------------
; Complex FFT, radix-1, decimation in place, 128 points
;
; Algorithm from "Numerical recipes in C", manually compiled in assembler
; and adapted...            
;
; Input : 128 complex values (each on 4 bytes), stored in data[0..511]
;         For each data : byte 0 = MSB of real part
;                         byte 1 = LSB of real part
;                         byte 2 = MSB of complex part
;                         byte 3 = LSB of complex part
;
; Replace data[0..511] by its FFT (128 frequencies, complex values)
;----------------------------------------------------------------------------


fft 	nop				; Control point : Input FFT

; ------> initialisation
	clrf j,F			; j=0
					; nn=128 (implicit), n=256 (implicit)

; ------> bit reversal
	clrf i,F			; for(i=0;i<n-1;i+=2) {
iloop1	nop

if1	movfp i,WREG			; if (j>i) {
	cpfsgt j
	goto endif1
    
	m_ldai j			; m_ldai(j)
	m_ldbi i			; m_ldbi(i)
	m_stai i			; m_stai(i)
	m_mvba				; m_mvba()
	m_stai j			; m_stai(j);
	incf j,F
	incf i,F
	m_ldai j			; m_ldai(j+1)
	m_ldbi i			; m_ldbi(i+1)
	m_stai i			; m_stai(i+1)
	m_mvba				; m_mvba()
	m_stai j			; m_stai(j+1)
	decf j,F
	decf i,F

endif1	nop				; } endif

    	movlw D'128'			; m=n>>1
	movwf m

while1	movlw D'2'			; while(m>=2 && j>=m) {
	cpfslt m
	goto $+2
	goto wend1	
	movfp m,WREG
	cpfslt j
	goto $+2
	goto wend1

 	movfp m,WREG			; j-=m
	subwf j,F

	bcf ALUSTA,C			; m>>=1
	rrcf m,F

wloop1	goto while1			; }

wend1	movfp m,WREG			; j+=m
	addwf j,F

inext1	incf i,F			; } next i
	incfsz i,F
	goto iloop1


; ------> FFT itself
  	movlw D'2'			; mmax=2
	movwf mmax

while2	tstfsz mmax			; while(n>mmax) {
	goto $+2
	goto wend2

	movfp mmax, WREG		; istep=mmax<<1
	bcf ALUSTA,C
	rlcf WREG,F
	movwf istep

    	; theta=2pi/mmax
	m_ldaconst H'6488'		; m_ldaconst(3.14159265459) 
	m_lddiv mmax			; m_lddiv(mmax>>1) 
	bcf ALUSTA,C
	rrcf rdiv,F
	call m_divi			; m_divi()
	m_sta theta			; m_sta(&theta)

	; wpr=-2*sin(theta/2)^2
	movlw D'2'			; m_lddiv(2)
	movwf rdiv
	call m_divi			; m_divi()
	call m_sin			; m_sin()
	m_mvab				; m_mvab()
	call m_mult			; m_mult()
	m_mvab				; m_mvab()
	m_ldaconst H'C000'		; m_ldaconst(-2.0)
	call m_mult			; m_mult()
	m_sta wpr			; m_sta(&wpr)

	; wpi=n_sin(theta)
	m_lda theta			; m_lda(theta)
	call m_sin			; m_sin()
	m_sta wpi			; m_sta(&wpi)

	; wr=1, wi=0
	m_ldaconst H'2000'		; m_ldaconst(1.0)
	m_sta wr			; m_sta(&wr)
	m_ldaconst H'0000'		; m_ldaconst(0.0)
	m_sta wi			; m_sta(&wi)

ffttst1	nop

	clrf m,F			; for(m=0;m<mmax-1;m+=2) {
mloop1	movfp mmax,WREG
	decf WREG,W
	cpfslt m
	goto mend1

ffttst2	nop

	movfp m,WREG			; for(i=m;i<n;i+=istep) {
	movwf i
iloop2	nop

	movfp i,WREG			; j=i+mmax
	movwf j
	movfp mmax,WREG
	addwf j,F

ffttst3	nop

	; h1r=wr*data[j]-wi*data[j+1]
	m_lda wi			; m_lda(wi)
	incf j,F			; m_ldbi(j+1)
	m_ldbi j
	decf j,F
	call m_mult			; m_mult()
	m_sta h1r			; m_sta(&h1r)
	m_lda wr			; m_lda(wr)
	m_ldbi j			; m_ldbi(j)
	call m_mult			; m_mult()
	m_ldb h1r			; m_ldb(h1r)
	m_sub				; m_sub()
	m_sta h1r			; m_sta(&h1r)

	; h1i=wr*data[j+1]+wi*data[j]
	m_lda wi			; m_lda(wi)
	m_ldbi j			; m_ldbi(j)
	call m_mult			; m_mult()
	m_sta h1i			; m_sta(&h1i)
	m_lda wr			; m_lda(wr)
	incf j,F			; m_ldbi(j+1)
	m_ldbi j
	decf j,F
	call m_mult			; m_mult()
	m_ldb h1i			; m_ldb(h1i)
	m_add				; m_add()
	m_sta h1i			; m_sta(&h1i)

	; butterfly update, with in-place scaling

	; data[j]=(data[i]-h1r)/2
	m_ldai i			; m_ldai(i)
	m_ldb h1r			; m_ldb(h1r)
	m_sub				; m_sub()
	m_div2				; m_div2()
	m_stai j			; m_stai(j)

	; data[j+1]=(data[i+1]-h1i)/2
	incf i,F			; m_ldai(i+1)
	m_ldai i
	decf i,F
	m_ldb h1i			; m_ldb(h1i)
	m_sub				; m_sub()
	m_div2				; m_div2()
	incf j,F			; m_stai(j+1)
	m_stai j
	decf j,F

	; data[i]=(data[i]+h1r)/2
	m_ldai i			; m_ldai(i)
	m_ldb h1r			; m_ldb(h1r)
	m_add				; m_add()
	m_div2				; m_div2()
	m_stai i			; m_stai(i)

	; data[i+1]=(data[i+1]+h1i)/2 
	incf i,F			; m_ldai(i+1)
	m_ldai i
	decf i,F
	m_ldb h1i			; m_ldb(h1i)
	m_add				; m_add()
	m_div2				; m_div2()
	incf i,F			; m_stai(i+1)
	m_stai i
	decf i,F

inext2	tstfsz istep			; } next i
	goto $+2
	goto iend2
	movfp istep,WREG	
	addwf i,F
	btfss ALUSTA,C
	goto iloop2

	; wtemp=wr
iend2	m_lda wr			; m_lda(wr)
	m_sta wtemp			; m_sta(&wtemp)

	; wr=wr*wpr-wi*wpi+wr
	m_lda wi			; m_lda(wi)
	m_ldb wpi			; m_ldb(wpi)
	call m_mult			; m_mult()
	m_sta h1r			; m_sta(&h1r)
	m_lda wr			; m_lda(wr)
	m_ldb wpr			; m_ldb(wpr)
	call m_mult			; m_mult()
	m_ldb h1r			; m_ldb(h1r)
	m_sub				; m_sub()
	m_ldb wr			; m_ldb(wr)
	m_add				; m_add()
	m_sta wr			; m_sta(&wr)

 	; wi=wi*wpr+wtemp*wpi+wi
	m_lda wtemp			; m_lda(wtemp)
	m_ldb wpi			; m_ldb(wpi)
	call m_mult			; m_mult()
	m_sta h1i			; m_sta(&h1i)
	m_lda wi			; m_lda(wi)
	m_ldb wpr			; m_ldb(wpr)
	call m_mult			; m_mult()
	m_ldb h1i			; m_ldb(h1i)
	m_add				; m_add()
	m_ldb wi			; m_ldb(wi)
	m_add				; m_add()
	m_sta wi			; m_sta(&wi)
    
mnext1	incf m,F			; } next m
	incf m,F
	goto mloop1
mend1	nop

	movfp istep,WREG		; mmax=istep
	movwf mmax

wloop2	goto while2			; } end while
wend2	return


;----------------------------------------------------------------------------
; Real FFT
;
; Algorithm from "Numerical recipes in C", manually compiled in assembler
; and adapted...            
;
; Calculate FFT of a set of 256 real values stored in data[0..511] 
; (2 bytes per value, MSB first).
;
; Replace these data by the positive frequency half of the FFT  
; (128 frequencies, complex values). First and last frequencies (which are 
; real) are in data[1] and data[2] respectively.
; 
; Method : consider the 256 real values as interleaved real/imaginary parts
; of 128 complex values. Do a complex 128 points FFT, and find back the 
; FFT of the real numbers from the output of the complex FFT...
;
; Nota : this method allow to do a real FFT with 30-40% less calculations
; and, more important, 50% less RAM requirement.
;----------------------------------------------------------------------------

realfft	nop				; control point, N=256 (implicit)

; ------> do a complex FFT of half the number of points, odd real points
;         as real data, even real points as imaginary data 

  	call fft

;-------> and find the good values from the result... 

    	; theta=2pi/n
	m_ldaconst H'6488'		; m_ldaconst(3.14159265459) 
	movlw D'128'			; m_lddiv(n>>1) 
	movwf rdiv
	call m_divi			; m_divi()
	m_sta theta			; m_sta(&theta)

	; wpr=-2*sin(theta/2)^2
	movlw D'2'			; m_lddiv(2)
	movwf rdiv
	call m_divi			; m_divi()
	call m_sin			; m_sin()
	m_mvab				; m_mvab()
	call m_mult			; m_mult()
	m_mvab				; m_mvab()
	m_ldaconst H'C000'		; m_ldaconst(-2.0)
	call m_mult			; m_mult()
	m_sta wpr			; m_sta(&wpr)

	; wpi=n_sin(theta)
	m_lda theta			; m_lda(theta)
	call m_sin			; m_sin()
	m_sta wpi			; m_sta(&wpi)

	; wr=1+wpr, wi=wpi
	m_ldb wpr			; m_ldb(wpr)
	m_ldaconst H'2000'		; m_ldaconst(1.0)
	m_add				; m_add()
	m_sta wr			; m_sta(&wr)
	m_lda wpi			; m_lda(wpi)
	m_sta wi			; m_sta(&wi)

	movlw D'2'			; for(i=2;i<=(n>>2);i++) {
	movwf i
iloop3	movlw D'64'
	cpfsgt i
	goto $+2
	goto iend3

	movfp i,WREG			; i1=i+i-2
	movwf i1
	addwf i1,F
	movlw D'2'
	subwf i1,F

	movfp i1,WREG			; i2=1+i1
	movwf i2
	incf i2,F

	movfp i2,WREG			; i3=n-i2+1
	comf WREG,W
	incf WREG,W
	incf WREG,W
	movwf i3

	incf WREG,W
	movwf i4			; i4=1+i3

	; h1r=(data[i1]+data[i3])/2
	m_ldai i1			; m_ldai(i1)
	m_ldbi i3			; m_ldbi(i3)
	m_add				; m_add()
	m_div2				; m_div2()
	m_sta h1r			; m_sta(&h1r)

	; h1i=(data[i2]-data[i4])/2
	m_ldai i2			; m_ldai(i2)
	m_ldbi i4			; m_ldbi(i4)
	m_sub				; m_sub()
	m_div2				; m_div2()
	m_sta h1i			; m_sta(&h1i)

	; h2r=(data[i2]+data[i4])/2
	m_ldai i2			; m_ldai(i2)
	m_ldbi i4			; m_ldbi(i4)
	m_add				; m_add()
	m_div2				; m_div2()
	m_sta h2r			; m_sta(&h2r)

	; h2i=-(data[i1]-data[i3])/2
	m_ldai i3			; m_ldai(i3)
	m_ldbi i1			; m_ldbi(i1)
	m_sub				; m_sub()
	m_div2				; m_div2()
	m_sta h2i			; m_sta(&h2i)

	; data[i1]=h1r+wr*h2r-wi*h2i
	m_lda wi			; m_lda(wi)
	m_ldb h2i			; m_ldb(h2i)
	call m_mult			; m_mult()
	m_sta wtemp			; m_sta(&wtemp)
	m_lda wr			; m_lda(wr)
	m_ldb h2r			; m_ldb(h2r)
	call m_mult			; m_mult()
	m_ldb wtemp			; m_ldb(wtemp)
	m_sub				; m_sub()
	m_ldb h1r			; m_ldb(h1r)
	m_add				; m_add()
	m_stai i1			; m_stai(i1)

	; data[i2]=h1i+wr*h2i+wi*h2r
	m_lda wi			; m_lda(wi)
	m_ldb h2r			; m_ldb(h2r)
	call m_mult			; m_mult()
	m_sta wtemp			; m_sta(&wtemp)
	m_lda wr			; m_lda(wr)
	m_ldb h2i			; m_ldb(h2i)
	call m_mult			; m_mult()
	m_ldb wtemp			; m_ldb(wtemp)
	m_add				; m_add()
	m_ldb h1i			; m_ldb(h1i)
	m_add				; m_add()
	m_stai i2			; m_stai(i2)

	; data[i3]=h1r-wr*h2r+wi*h2i
	m_lda wr			; m_lda(wr)
	m_ldb h2r			; m_ldb(h2r)
	call m_mult			; m_mult()
	m_sta wtemp			; m_sta(&wtemp)
	m_lda wi			; m_lda(wi)
	m_ldb h2i			; m_ldb(h2i)
	call m_mult			; m_mult()
	m_ldb wtemp			; m_ldb(wtemp)
	m_sub				; m_sub()
	m_ldb h1r			; m_ldb(h1r)
	m_add				; m_add()
	m_stai i3			; m_stai(i3)

	; data[i4]=-h1i+wr*h2i+wi*h2r
	m_lda wr			; m_lda(wr)
	m_ldb h2i			; m_ldb(h2i)
	call m_mult			; m_mult()
	m_sta wtemp			; m_sta(&wtemp)
	m_lda wi			; m_lda(wi)
	m_ldb h2r			; m_ldb(h2r)
	call m_mult			; m_mult()
	m_ldb wtemp			; m_ldb(wtemp)
	m_add				; m_add()
	m_ldb h1i			; m_ldb(h1i)
	m_sub				; m_sub()
	m_stai i4			; m_stai(i4)

	; wtemp=wr 
	m_lda wr			; m_lda(wr)
	m_sta wtemp			; m_sta(&wtemp)

	; wr=wr*wpr-wi*wpi+wr
	m_lda wi			; m_lda(wi)
	m_ldb wpi			; m_ldb(wpi)
	call m_mult			; m_mult()
	m_sta h1r			; m_sta(&h1r)
	m_lda wr			; m_lda(wr)
	m_ldb wpr			; m_ldb(wpr)
	call m_mult			; m_mult()
	m_ldb h1r			; m_ldb(h1r)
	m_sub				; m_sub()
	m_ldb wr			; m_ldb(wr)
	m_add				; m_add()
	m_sta wr			; m_sta(&wr)

	; wi=wi*wpr+wtemp*wpi+wi
	m_lda wi			; m_lda(wi)
	m_ldb wpr			; m_ldb(wpr)
	call m_mult			; m_mult()
	m_sta h1r			; m_sta(&h1r)
	m_lda wtemp			; m_lda(wtemp)
	m_ldb wpi			; m_ldb(wpi)
	call m_mult			; m_mult()
	m_ldb h1r			; m_ldb(h1r)
	m_add				; m_add()
	m_ldb wi			; m_ldb(wi)
	m_add				; m_add()
	m_sta wi			; m_sta(&wi)


inext3	incf i,F				; } next i
	goto iloop3
iend3	nop

; ------> special case of the 2 first data

	; h1r=data[0]
	movlw 0				; m_ldai(0)
	movwf i
	m_ldai i
	m_sta h1r			; m_sta(&h1r)

	; data[0]=h1r+data[1]
	movlw 1				; m_ldbi(1)
	movwf i
	m_ldbi i
	m_add				; m_add()
	movlw 0				; m_stai(0)
	movwf i
	m_stai i

	; data[1]=h1r-data[1]
	m_lda h1r			; m_lda(h1r)
	movlw 1				; m_ldbi(1)
	movwf i
	m_ldbi i
	m_sub				; m_sub()
	movlw 1				; m_stai(1)
	movwf i
	m_stai i

endrft	return



;============================================================================
;                             - END OF FILE -
;============================================================================