' ******************************************************************
' Fast Fourier Transform
' Modified from a Pascal program by Don Cross <dcross@intersrv.com>
' ******************************************************************

' ------------------------------------------------------------------
' Error codes
' ------------------------------------------------------------------

#DEFINE FFTOk 0  
' No error

#DEFINE FFTPowErr -1 
' Number of samples is not a power of 2

#DEFINE FFTBoundErr -2 
' Arrays do not start at index 0

' ------------------------------------------------------------------
' Mathematical constant
' ------------------------------------------------------------------

#DEFINE TwoPi 6.283185307179586#
' 2*Pi

' ------------------------------------------------------------------
' Type definition
' ------------------------------------------------------------------

TYPE Complex
  X AS DOUBLE
  Y AS DOUBLE
END TYPE

' ------------------------------------------------------------------
' Global variable
' ------------------------------------------------------------------

COMMON SHARED ErrCode AS INTEGER
' Error code from the last function evaluation

' ******************************************************************

#DEFINE MaxPower 32

FUNCTION IsPowerOfTwo(X AS INTEGER) AS INTEGER

  DIM AS INTEGER I, Y

  Y = 2
  FOR I = 1 TO MaxPower - 1
    IF X = Y THEN IsPowerOfTwo = -1 : EXIT FUNCTION
    Y = Y SHL 1
  NEXT I
  IsPowerOfTwo = 0
END FUNCTION

FUNCTION NumberOfBitsNeeded(PowerOfTwo AS INTEGER) AS INTEGER
  
  DIM I AS INTEGER

  FOR I = 0 TO MaxPower
    IF (PowerOfTwo AND (1 SHL I)) <> 0 THEN
      NumberOfBitsNeeded = I
      EXIT FUNCTION
    END IF
  NEXT I
  NumberOfBitsNeeded = 0
END FUNCTION

FUNCTION ReverseBits(Index AS INTEGER, NumBits AS INTEGER) AS INTEGER
  
  DIM AS INTEGER I, K, Rev

  Rev = 0
  I = Index
  FOR K = 0 TO NumBits - 1
    Rev = (Rev SHL 1) OR (I AND 1)
    I = I SHR 1
  NEXT K
  ReverseBits = Rev
END FUNCTION

SUB FourierTransform(AngleNumerator AS DOUBLE,  _
                     InArray()      AS Complex, _
                     OutArray()     AS Complex) 

  IF LBOUND(InArray) <> 0 OR LBOUND(OutArray) <> 0 THEN
    ErrCode = FFTBoundErr
    EXIT SUB
  END IF

  DIM AS INTEGER NumSamples
  
  NumSamples = UBOUND(InArray) + 1
  IF (NOT IsPowerOfTwo(NumSamples)) OR (NumSamples < 2) THEN
    ErrCode = FFTPowErr
    EXIT SUB
  END IF

  DIM AS INTEGER NumBits, I, J, K, N, BlockSize, BlockEnd
  DIM AS DOUBLE  Delta_angle, Delta_ar, Alpha, Beta, Tr, Ti, Ar, Ai

  ErrCode = FFTOk
  
  NumBits = NumberOfBitsNeeded(NumSamples)
  FOR I = 0 TO NumSamples - 1
    J = ReverseBits(I, NumBits)
    OutArray(J).X = InArray(I).X
    OutArray(J).Y = InArray(I).Y
  NEXT I

  BlockEnd = 1
  BlockSize = 2

  DO WHILE BlockSize <= NumSamples
    Delta_angle = AngleNumerator / BlockSize
    Alpha = SIN(0.5 * Delta_angle)
    Alpha = 2 * Alpha * Alpha
    Beta = SIN(Delta_angle)

    I = 0
    DO WHILE I < NumSamples
      Ar = 1  ' cos(0)
      Ai = 0  ' sin(0)

      J = I
      FOR N = 0 TO BlockEnd - 1
        K = J + BlockEnd
        Tr = Ar * OutArray(K).X - Ai * OutArray(K).Y
        Ti = Ar * OutArray(K).Y + Ai * OutArray(K).X
        OutArray(K).X = OutArray(J).X - Tr
        OutArray(K).Y = OutArray(J).Y - Ti
        OutArray(J).X = OutArray(J).X + Tr
        OutArray(J).Y = OutArray(J).Y + Ti
        Delta_ar = Alpha * Ar + Beta * Ai
        Ai = Ai - (Alpha * Ai - Beta * Ar)
        Ar = Ar - Delta_ar
        J = J + 1
      NEXT N

      I = I + BlockSize
    LOOP

    BlockEnd = BlockSize
    BlockSize = BlockSize SHL 1
  LOOP
END SUB

SUB FFT(InArray() AS Complex, OutArray() AS Complex)
' ------------------------------------------------------------------
' Calculates the Fast Fourier Transform of the array of
' complex numbers 'InArray' to produce the output complex
' numbers in 'OutArray'
' ------------------------------------------------------------------

  FourierTransform TwoPi, InArray(), OutArray()
END SUB

SUB IFFT(InArray() AS Complex, OutArray() AS Complex)
' ------------------------------------------------------------------
' Calculates the Inverse Fast Fourier Transform of the array of
' complex numbers represented by 'InArray' to produce the output
' complex numbers in 'OutArray'
' ------------------------------------------------------------------
  
  FourierTransform -TwoPi, InArray(), OutArray()

  IF ErrCode <> FFTOk THEN EXIT SUB
  
  DIM AS INTEGER NumSamples, I
  
  NumSamples = UBOUND(InArray) + 1
  
  ' Normalize the resulting time samples 
  FOR I = 0 TO NumSamples - 1
    OutArray(I).X = OutArray(I).X / NumSamples
    OutArray(I).Y = OutArray(I).Y / NumSamples
  NEXT I
END SUB

FUNCTION CalcFrequency(FrequencyIndex AS INTEGER, _
                       InArray()      AS Complex) AS Complex
' ------------------------------------------------------------------
' This function calculates the complex frequency sample at a given
' index directly. Use this instead of 'FFT' when you only need one
' or two frequency samples, not the whole spectrum.
'
' It is also useful for calculating the Discrete Fourier Transform
' (DFT) of a number of data which is not an integer power of 2.
' For example, you could calculate the DFT of 100 points instead
' of rounding up to 128 and padding the extra 28 array slots with
' zeroes.
' ------------------------------------------------------------------

  IF LBOUND(InArray) <> 0 THEN
    ErrCode = FFTBoundErr
    EXIT FUNCTION
  END IF

  ErrCode = FFTOk
  
  DIM AS INTEGER NumSamples, K
  DIM AS DOUBLE  Cos1, Cos2, Cos3, Theta, Beta, Sin1, Sin2, Sin3
  DIM AS Complex Res

  NumSamples = UBOUND(InArray) + 1
  
  Res.X = 0
  Res.Y = 0

  Theta = TwoPi * FrequencyIndex / NumSamples
  Sin1  = SIN(- 2 * Theta)
  Sin2  = SIN(- Theta)
  Cos1  = COS(- 2 * Theta)
  Cos2  = COS(- Theta)
  Beta  = 2 * Cos2

  FOR K = 0 TO NumSamples - 1
    ' Update trig values
    Sin3 = Beta * Sin2 - Sin1
    Sin1 = Sin2
    Sin2 = Sin3

    Cos3 = Beta * Cos2 - Cos1
    Cos1 = Cos2
    Cos2 = Cos3

    Res.X = Res.X + InArray(K).X * Cos3 - InArray(K).Y * Sin3
    Res.Y = Res.Y + InArray(K).Y * Cos3 + InArray(K).X * Sin3
  NEXT K
  
  CalcFrequency = Res
END FUNCTION
