;-------------------------------------------Scaled Phase Locking Phase vocoder---------------------------------------------------------------
(bind-func phase_vocoder_sPL
(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))))
(prev_in_phase:float* (zalloc spectrum_size))
(prev_out_phase:float* (zalloc spectrum_size))
(omega_k:float* (zalloc spectrum_size)) ;The center frequency of the kth vocoder channel
(prev_peaks_array:i64* (zalloc peaks_size))
(prev_region_lower:i64* (zalloc peaks_size))
(prev_region_upper:i64* (zalloc peaks_size))
(prev_num_peaks:i64 0))
(dotimes (idx spectrum_size)
(pset! prev_in_phase idx 0.0)
(pset! prev_out_phase idx 0.0)
(pset! omega_k idx (/ (* TWOPIf (i64tof idx))
(i64tof buffer_size)))) ;spectrum_size => acts as low pass filter. buffer size => normal
(dotimes (idx peaks_size)
(pset! prev_peaks_array idx 0)
(pset! prev_region_lower idx 0)
(pset! prev_region_upper idx 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))
(mag_adj: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))
(delta_phi_s:float* (salloc spectrum_size))
(k_s:float* (salloc spectrum_size))
(delta_phi_adjust_s:float* (salloc spectrum_size))
(inst_freq_s:float* (salloc spectrum_size))
(synth_phase_s: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))
(prev_peak:i64 0)
(difference:float* (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) ;number of current peaks
(dotimes (n num_peaks)
(set! prev_peak (find_previous_peak (pref peaks_array n) prev_peaks_array prev_region_lower prev_region_upper peaks_size))
(cond ((or (= prev_peak -1) (> (fabs (i64tof (- (pref peaks_array n) prev_peak))) (* (i64tof(pref peaks_array n)) range))) ;no previous peaks or outside allowable range
;iPL phase vocoder if no previous peaks, or the previous peak is too far away
;(printf "Current=%d, previous=%d" (pref peaks_array n) prev_peak) ;uncomment these to see how setting the range affects the algorithm used
;(println (fabs (i64tof (- (pref peaks_array n) prev_peak))))
(pset! delta_phi_s 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_s n (round (/ (pref delta_phi_s n) TWOPIf)))
(pset! delta_phi_adjust_s n (- (pref delta_phi_s n)
(* (pref k_s n) TWOPIf))) ;Adjust to -pi<phase<pi
(pset! inst_freq_s n (+ (pref omega_k (pref peaks_array n)) (/ (pref delta_phi_adjust_s n) (i64tof Sa)))) ;Calculate the Instantaneous Frequency
(pset! synth_phase_s n (+ (pref prev_out_phase (pref peaks_array n)) (* (i64tof Ss) (pref inst_freq_s 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_s n) (pref phase (pref peaks_array n))))
(pset! synth_phase (+ l (pref region_lower n)) (+ (pref phase (+ l (pref region_lower n)))
(pref theta (+ l (pref region_lower n)))))))
(else
;sPL phase vocoder when a previous peak is found within the acceptable region
(pset! delta_phi_s n (- (pref phase (pref peaks_array n))
(pref prev_in_phase prev_peak)
(* (i64tof Sa) (pref omega_k (pref peaks_array n)))));Calculate the Instantaneous Phase
(pset! k_s n (round (/ (pref delta_phi_s n) TWOPIf)))
(pset! delta_phi_adjust_s n (- (pref delta_phi_s n)
(* (pref k_s n) TWOPIf)));Adjust to -pi<phase<pi
(pset! inst_freq_s n (+ (pref omega_k (pref peaks_array n)) (/ (pref delta_phi_adjust_s n) (i64tof Sa))));Calculate the Instantaneous Frequency
(pset! synth_phase_s n (+ (pref prev_out_phase prev_peak) (* (i64tof Ss) (pref inst_freq_s n))));Calculate the new Synthesis phase
(dotimes (l (+ 1 (- (pref region_upper n)(pref region_lower n))))
;Calculate the difference between the peak channel and all other channels in the current region
(pset! difference (+ l (pref region_lower n)) (- (pref phase (+ l (pref region_lower n)))
(pref phase (pref peaks_array n))))
;Calculate the Synthesis phase for each bin
(pset! synth_phase (+ l (pref region_lower n)) (+ (pref synth_phase_s n)
(* beta (pref difference (+ l (pref region_lower n))))))))))
(dotimes (n spectrum_size) ;account for loss of volume during PV.
(pset! mag_adj n (* (pref mag n) 1.2)))
(Complex_bufferize mag_adj synth_phase spectrum spectrum_size));Create spectrum
(else
(Complex_bufferize mag phase spectrum spectrum_size)))
(if (= speed 1.0) ;out=in at speed of 1
(Complex_bufferize mag phase spectrum spectrum_size))
;Store current peaks, region bounds, in phase and out phase for use in the next frame analysis
(dotimes (n spectrum_size)
(pset! prev_in_phase n (pref phase n))
(pset! prev_out_phase n (tref (pref-ptr spectrum n) 1)))
(dotimes (n peaks_size)
(pset! prev_peaks_array n 0)
(pset! prev_region_lower n 0)
(pset! prev_region_upper n 0))
(dotimes (n num_peaks)
(pset! prev_peaks_array n (pref peaks_array n))
(pset! prev_region_upper n (pref region_upper n))
(pset! prev_region_lower n (pref region_lower n)))
(set! prev_num_peaks num_peaks)
(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. wn is already a hanning window
void))))) ;Overlap and add this to the output