phase_vocoder_iPL   xtlang

;--------------------------------------------------------------------------------------------------------------------
;-------------------------------Identity Phase Locking Phase vocoder-------------------------------------------------
(bind-func phase_vocoder_iPL
  (lambda (buffer_size:i64)
    (let ((idx:i64 0)
          (spectrum_size:i64 (+ (/ buffer_size 2) 1))
          (peaks_size:i64 (ftoi64 (ceil (/ (i64tof spectrum_size) 3.0)))) ; Assuming peak is greater than 4 neighbours
          (prev_in_phase:float* (zalloc spectrum_size)) ;Need to init this to 0
          (prev_out_phase:float* (zalloc spectrum_size)) ;Need to init this to 0
          (omega_k:float* (zalloc spectrum_size)) ;The center frequency of the kth vocoder channel
          (unity_mag:float* (zalloc spectrum_size)))
    (dotimes (idx (+ (/ buffer_size 2) 1))
      (pset! prev_in_phase idx 0.0)
      (pset! prev_out_phase idx 0.0)
      (pset! omega_k idx (/ (* TWOPIf (i64tof idx))
                            (i64tof buffer_size)))
      (pset! unity_mag idx 1.0))
    (lambda (buffer:float* Sa:i64 Ss:i64)
      (let ((temp_buff:float* (salloc buffer_size))
            (wn:float* (salloc buffer_size))
            (spectrum:Complexf* (salloc spectrum_size))
            (mag:float* (salloc spectrum_size))
            (phase:float* (salloc spectrum_size))
            (delta_phi:float* (salloc spectrum_size))
            (k:float* (salloc spectrum_size))
            (delta_phi_adjust:float* (salloc spectrum_size))
            (inst_freq:float* (salloc spectrum_size))
            (synth_phase:float* (salloc spectrum_size))
            (n:i64 0)
            (l:i64 0)
            (num_peaks:i64 0)
            (peaks_array:i64* (salloc peaks_size))
            (region_lower:i64* (salloc peaks_size))
            (region_upper:i64* (salloc peaks_size))
            (theta:float* (salloc spectrum_size))
            (Z:Complexf* (salloc spectrum_size))
            (spectrum_polar:Complexf* (salloc spectrum_size)))
      (hanning_window_func wn buffer_size)
      (vvmul buffer wn buffer_size temp_buff) ;Apply window function
      (vrotate temp_buff buffer_size (/ buffer_size 2)) ;Circular shift the windowed frame
      (fft temp_buff spectrum buffer_size) ;Compute the DFT of windowed frame
      (dotimes (n spectrum_size) ;Compute the magnitude spectrum
        (pset! mag n (Complex_mag (pref spectrum n))))
      (dotimes (n spectrum_size) ;Compute the phase spectrum
        (pset! phase n (Complex_phase2 (pref spectrum n))))
      (set! num_peaks (find_peaks mag spectrum_size peaks_array region_lower region_upper)) ;Find the peaks and region limits in the magnitude spectrum
      (cond ((> num_peaks 0)
              (dotimes (n num_peaks)
              ;Unwrap the phase at each peak
                (pset! delta_phi n (- (pref phase (pref peaks_array n))
                                      (pref prev_in_phase (pref peaks_array n))
                                      (* (i64tof Sa) (pref omega_k (pref peaks_array n))))) ;Calculate the Instantaneous Phase
                (pset! k n (round (/ (pref delta_phi n) TWOPIf)))
                (pset! delta_phi_adjust n (- (pref delta_phi n)
                                             (* (pref k n) TWOPIf))) ;Adjust to -pi<phase<pi
                (pset! inst_freq n (+ (pref omega_k (pref peaks_array n)) (/ (pref delta_phi_adjust n) (i64tof Sa)))) ;Calculate the Instantaneous Frequency
                (pset! synth_phase n (+ (pref prev_out_phase (pref peaks_array n)) (* (i64tof Ss) (pref inst_freq n)))) ;Calculate the new Synthesis phase
                (dotimes (l (+ 1 (- (pref region_upper n) (pref region_lower n)))) ;Calculate the rotation angle (theta)
                  (pset! theta (+ l (pref region_lower n)) (- (pref synth_phase n) (pref phase (pref peaks_array n))))))
              (Complex_bufferize unity_mag theta Z spectrum_size) ;Calculate Phasor (Z=e^i*theta)
              (Complex_bufferize mag phase spectrum_polar spectrum_size)
              (Complex_multiplication_polar Z spectrum_polar spectrum spectrum_size) ;Apply the Phasor to all channels in the region of the peak
              (dotimes (n spectrum_size) ;Set previous input and output phase ready for next frame
                (pset! prev_in_phase n (pref phase n))
                (pset! prev_out_phase n (tref (pref-ptr spectrum n) 1))))
            (else
              (Complex_bufferize mag phase spectrum spectrum_size)))
      (if (= speed 1.0) ;out=in at speed of 1
        (begin
          (dotimes (n spectrum_size)
            (pset! synth_phase n (pref phase n)))
          (Complex_bufferize mag synth_phase spectrum spectrum_size)))
      (pol_to_cart2 spectrum spectrum_size) ;Convert from polar back to cartesian
      (ifft spectrum temp_buff buffer_size) ;take the real part of iFFT
      (vsdiv temp_buff (i64tof buffer_size) buffer_size temp_buff) ;this could be changed to a dotimes
      (vrotate temp_buff buffer_size (/ buffer_size 2)) ;Circular shift the time domain Output
      (vvmul temp_buff wn buffer_size buffer) ;Apply output window function
      void))))) ;Overlap and add this to the output