;; spectrum.lsp -- operations on spectral frames

;; ALL OF THESE FUNCTIONS SEEM TO BE SUPERCEDED BY spectral-analysis.lsp
;; USE THAT INSTEAD. THIS IS HERE FOR COMPATIBILITY WITH ANY OLD CODE THAT
;; NEEDS IT.

;; Update 2021: functions for FFT, callable from SAL or LISP
;;
;;    make-fft-iterator(sound, length, skip) [SAL]
;;    (make-fft-iterator sound length skip) [LISP]
;;        returns an object the returns FFTs
;;
;;    next-fft(iterator) [SAL]
;;    (next-fft iterator) [LISP]
;;        iterator - an object returned by make-fft-iterator
;;        returns next FFT as an array or NIL at end of sound
;;        *rslt* is set to the time of the center of the FFT window
;;
;;    spectrum-to-amplitude(fft) [SAL]
;;    (spectrum-to-amplitude fft) [LISP]
;;        fft - an array returned by next-fft
;;        returns magnitude spectrum
;;
;;    bin-frequency(bin, srate, length) [SAL]
;;    (bin-frequency bin srate length) [LISP]
;;        bin - a bin number
;;        srate - sample rate of sound under analysis
;;        length - length of the FFT
;;        returns center frequency for bin
;;        NOTE: bin refers to the complex bin number or the array
;;            index of a magnitude spectrum. To get the frequency
;;            for the ith element of a full complex spectrum, use
;;            bin = (/ (1+ 1) 2), or in SAL bin = (i + 1) / 2
;;
;;    spectrum-print(file, frame [, srate: srate] [, cutoff: cutoff]
;;                   [, threshold: threshold]) [SAL]
;;    (spectrum-print file frame [:srate srate] [:cutoff cutoff]
;;                    [:threshold threshold])  [LISP]
;;        srate - sample rate of analyzed sound
;;                (default is *default-sound-srate*)
;;        cutoff - highest frequency to plot
;;        threshold - if more than 3 adjacent bins have values below the
;;                    threshold, the first and last are printed, and a
;;                    dashed line is printed between them to elide many
;;                    bins with no significant amplitude.
;;        print a complex spectrum with ascii plot, including the bin
;;            number, frequency of the bin, and magnitude.
;;        
;;    amplitude-print(file, frame [, srate]) [SAL]
;;    (amplitude-print file frame [srate]) [LISP]
;;        file - output file, can be T for stdout
;;        frame - a magnitude spectrum
;;        srate - sample rate of the original file
;;                (defaults to *default-sound-srate*)
;;        print a magnitude spectrum with ascii plot
;;
;;   amplitude-normalize(frame [, max]) [SAL]
;;   (amplitude-normalize frame max) [LISP]
;;        frame - an array
;;        max - maximum for output
;;        returns a new array scaled so that the maximum value is max
;;                (default for max is 1). If the array is all zero,
;;                no scaling takes place.


(defun raised-cosine ()
  (scale 0.5 
    (sum (const 1) 
         (lfo (/ 1.0 (get-duration 1)) 1 *sine-table* 270))))

(defun fft-window (frame-size)
  (control-srate-abs frame-size (raised-cosine)))

;; fft-class -- an iterator converting sound to sequence of frames
;;
(setf fft-class (send class :new '(sound length skip window offset)))

(send fft-class :answer :next '() '(
  (let ((frame (snd-fft sound length skip window)))
    (setf *rslt* (/ offset (snd-srate sound)))
    (setf offset (+ offset skip))
    frame)))

(defun next-fft (iterator) (send iterator :next))

(defun bin-frequency (bin srate length)
  (* bin (/ (float srate) length)))
  
(send fft-class :answer :isnew '(snd len skp) '(
    (setf sound snd)
    (setf length len)
    (setf skip skp)
    (setf offset (/ length 2)) ;; count in samples
    (setf window (fft-window len)) ))


(defun make-fft-iterator (sound length skip)
  (send fft-class :new (snd-copy sound) length skip))


;; conversions -- assumes frame length is even

(defun spectrum-to-amplitude (frame)
  (let* ((n (length frame))
         (half-n (/ n 2))
         (amps (make-array (1+ half-n))))
    (setf (aref amps 0) (abs (aref frame 0)))
    (dotimes (i (1- half-n))
      (let* ((i2 (+ i i))
             (c (aref frame (1+ i2)))
             (s (aref frame (+ 2 i2))))
        (setf (aref amps (1+ i))
              (sqrt (+ (* c c) (* s s))))))
    (setf (aref amps half-n) (abs (aref frame (1- n))))
    amps))

(defun spectrum-by-amplitude (frame amps)
  (let* ((n (length frame))
         (half-n (/ n 2)))
    (setf (aref frame 0) (* (aref frame 0) (aref amps 0)))
    (dotimes (i (1- half-n))
      (let* ((ip1 (1+ i))
             (i2 (+ i i))
             (i2p1 (1+ i2))
             (i2p2 (1+ i2p1)))
        (setf (aref frame i2p1) (* (aref frame i2p1)
                                   (aref amps ip1)))
        (setf (aref frame i2p2) (* (aref frame i2p2)
                                   (aref amps ip1)))))
    (setf (aref frame (1- n)) (* (aref frame (1- n))
                                 (aref amps half-n)))))


(defun spectrum-rms (frame)
  (let* ((n (length frame))
         (half-n (/ n 2))
         (sum (* (aref frame 0) (aref frame 0))))
    (dotimes (i (1- half-n))
      (let* ((i2 (+ i i))
             (c (aref frame (1+ i2)))
             (s (aref frame (+ 2 i2))))
        (setf sum (+ sum (* c c) (* s s)))))
    (setf sum (+ sum (* (aref frame (1- n)) (aref frame (1- n)))))
    (sqrt sum)))


(defun amplitude-rms (frame)
  (let* ((n (length frame))
         (sum 0))
    (dotimes (i n)
      (setf sum (+ sum (* (aref frame i) (aref frame i)))))
    (sqrt sum)))

;; SMOOTH-AMPLITUDE -- simple local averaging to smooth out
;;   an amplitude spectrum. This might be useful to broaden
;;   spectral peaks from partials to better represent vocal 
;;   formants. It would be nice to have a "width" parameter,
;;   but instead, the filter is fixed at (0.25, .5, 0.25)
;;
(defun smooth-amplitude (frame)
  (let* ((len (length frame))
         (lenm1 (1- len))
         (lenm2 (1- lenm1))
         (rslt (make-array (length frame))))
    (setf (aref rslt 0) (+ (* 0.75 (aref frame 0))
                           (* 0.25 (aref frame 1))))
    (dotimes (i lenm2)
      (let* ((ip1 (1+ i))
             (ip2 (1+ ip1)))
        (setf (aref rslt ip1) (+ (* 0.25 (aref frame i))
                                 (* 0.5 (aref frame ip1))
                                 (* 0.25 (aref frame ip2))))))
    (setf (aref rslt lenm1) (+ (* 0.25 (aref frame lenm2))
                               (* 0.75 (aref frame lenm1))))
    rslt))

;; ARRAY-SCALE -- multiply a spectral frame or amplitude frame
;;  by a scale factor, returns the frame
;;
(defun array-scale (frame x)
  (dotimes (i (length frame))
    (setf (aref frame i) (* (aref frame i) x)))
  frame)

(defun amplitude-normalize (frame &optional (max 1.0))
  (let ((peak 0.0))
    (dotimes (i (length frame))
      (setf peak (max (aref frame i) peak)))
    (setf frame (array-copy frame))
    (if (> peak 0)
        (array-scale frame (/ max peak)))
    frame))


(defun array-copy (frame)
  (let* ((len (length frame))
         (copy (make-array len)))
    (dotimes (i len)
      (setf (aref copy i) (aref frame i)))
    copy))


(defun amplitude-plot (frame &optional (srate *default-sound-srate*))
  (s-plot (snd-from-array 0 
            (/ (float (1- (length frame))) srate)
            frame)))


(defun spectrum-plot (frame &optional (srate *default-sound-srate*))
  (amplitude-plot (spectrum-to-amplitude frame) srate))


(defun spectrum-ifft (iterator len skip)
  (snd-ifft (local-to-global 0.0) *sound-srate* iterator skip (fft-window len)))

(defun spectrum-print (file frame &optional (srate *default-sound-srate*))
  (amplitude-print file (spectrum-to-amplitude frame) srate))


(defun ap-compute-next-to-print (frame i threshold last-bin)
  ;; look beyond i and see if we should skip some lines
  (cond ((or (< (aref frame i) threshold)
             (>= i last-bin))
         (prog ((j (1+ i)))
           loop
           (cond ((>= j (length frame)) (go done))
                 ((or (< (aref frame j) threshold)
                      (>= j last-bin))
                  (setf j (1+ j))
                  (go loop)))
           done
           ;; now i is potentially the beginning of a run
           ;; and j is potentially ap-end-skip (> threshold)
           ;; we only skip if the run is longer than 3, so
           ;; j - i > 3:
           (cond ((> (- j i) 3)
                  ;; recall that ap-start-skip is the 2nd bin of the run:
                  (setf ap-start-skip (1+ i))
                  (setf ap-end-skip j)))))))


(defun amplitude-print (file frame &key (srate *default-sound-srate*)
                             (cutoff nil) (threshold -1))
  (format file "~A~%" "  BIN    FREQ   MAG")
  ;; algorithm: ap-start-skip is first line to skip, print "-------" here
  ;;   ap-end-skip is one beyond last line to skip, print this one and beyond
  ;;   when beyond, call ap-compute-next-to-print to search ahead for new
  ;;   bins to skip and set ap-start-skip, ap-end-skip. These are globals.
  (setf ap-start-skip -10 ap-end-skip -10)
  (let ((last-bin (if cutoff
                      (truncate (* (length frame) 2.0 (/ cutoff (float srate))))
                    (length frame))))
    (setf last-bin (min (length frame) last-bin)) ;; because it stops the loop
                                                  ;; in ap-compute-next-to-print
    (progv '(*float-format* *integer-format*) '("%7.1f" "%5d")
      (dotimes (i (length frame))
        (cond ((and (> i ap-start-skip) (< i ap-end-skip))
               nil) ;; skip the bin
              ((= i ap-start-skip)
               (format file "--------------------~%"))
              (t
               (ap-compute-next-to-print frame i threshold last-bin)
               (setf *float-format* "%7.1f")
               (format file "~A ~A " i (bin-frequency i srate
                                         (* 2 (1- (length frame)))))
               (setf *float-format* "%5.3f")
               (format file "~A: " (aref frame i))
               (dotimes (j (round (* 50 (aref frame i))))
                 (format file "*"))
               (format file "~%")))))))

                
