%
%A Digital Implementation of the Buchla Low Pass Gate based on the
%Parker-D'angelo Model
%Josh Rohs
%2018
clear all
close all

%User Parameters
OS=2; %Oversampling factor
Fs = OS*44.1E3; %sampling frequency
N = Fs*4; %Number of Samples to compute
mode = 3; %Lopass
%mode = 2; %VCA
%mode = 1; % Both
input_filename = 'square2.aif';
cv = "modsweep"; %sweep, trigger, accented, sine, zero, modsweep.
modDepth =8; %V. Clipping begins at ~10.7V
offset =0;

anorm = .5 ; %Normalized Feedback. will oscillate for a=1.0 in linear mode. 
tau_on = 0.012; %Vactrol Attack Time
tau_off = .25; %Vactrol Decay Time
rho_on = .9;
rho_off = 0.9;
nonlin = "false";
smoothed = "true";
nonlin_drive =1.0; 
save = false; %Saves output graphs and writes wavfiles to Results folder
%% End user parameter inputs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%vactrol impulse response cutoffs
wc_on = 5/tau_on;
wc_off = 5/tau_off;
B_vac = 1136.2129956;% ohms, Light Resistance. From Parker & D'Angelo
A_vac = 3.4645912;% ohm*amps^1.4. From Parker & D'Angelo
%Create resulsts directory
if (7~=exist("Results",'dir'))
    mkdir("Results");
end
%Read, scale and upsample input
input_file = audioread(input_filename);
input_file = input_file(:,1);
input_file = input_file./max(input_file)*.9; %normalize file
input_file = resample(input_file,OS,1);
output_filename = getFilename(input_filename,mode,cv,nonlin,smoothed);
%Initialize Vectors
dt = 1/Fs; %time step size
t = [0:dt:N*dt-dt];
cv_in=getCv(cv,t,modDepth); %Get cv in vector from getCv function
output = zeros(1,N);
If = zeros(1,N);
If_v = zeros(1,N); % Current after being processed by nonlinear vactrol filter model. 
%%
%%Control Path
%start control path simulation
sc = 0;
yc = 0;
x_m1 = 0;
dir_n1 = 2; %set to make sure it evaluates the first time
wc = wc_on;
for i=1:N
    %Detect CV signal increasing or decreasing
    If(i)=getCurrent(cv_in(i),offset,smoothed);
    dir = sign(If(i)-x_m1);
     if dir ~= dir_n1 %Only update when input direction is changed
        switch(dir)
                    case 1
                         wc = wc_on*(1-rho_on+rho_on*If(i)/.040); 
                         % Use incoming cv_in value to scale cutoff based on max curent (40mA)
                    case 0 
                         %wc = wc;
                    case -1
                         wc = wc_off*(1+rho_off-(rho_off)*(yc)/0.040) ;
                         % use outgoing yo value to scale cutoff based on max current
                    otherwise
                        display("This shouldn't print");
                    end
     end
    %Set filter gain/cutoff based on attack/decay
    %Calculate yc and sc based on trapezoidal 1-pole filter model
    g = wc*dt/2;    
    vc = (If(i)-sc)*g/(1+g);
    yc = vc + sc;
    sc = yc + vc;
    
    %update processed current
    x_m1 = If(i);
    dir_n1 = dir;
    If_v(i) = yc;
    If_v(i)=max( If_v(i),1.01E-5);
        

end
Rf = 1./(If_v.^1.4)*A_vac+B_vac;


%% Process Audio
%Start audio path simulation
% set component values
C1 = 1E-9;
C2 = 220E-12;
if (mode == 3)
    C3 = 4.7E-9;
else
    C3 = 0;
    anorm=0.0;
end
 if (mode == 2)
    Ra = 5E3;
else
    Ra = 5E6;
end   

b4 = C3/C2;
sx=0;
sd=0;
so=0;
yo = 0;
xo=0; %used for nonlinear model
d2= -1;
j=1;
for i=1:N
%calculate transfer function coefficients
    if mode ~=3
        a=0;
    else
        amax = (2*C1*Ra+(C2+C3)*(Ra+Rf(i)))/(C3*Ra);
        a = anorm*(amax);  
    end
    a1 = 1/(Rf(i)*C1);
    a2 = -1/C1*(1/Rf(i)+1/Ra);
    b1 = 1/(Rf(i)*C2);
    b2 = -2/(Rf(i)*C2);
    b3 = b1;
    d1 = a;

    %Read input file. Loop if finished!
    if j < length(input_file)     
        j =j+1;
    else
        j=1;
    end
        yi = input_file(j);
    %Solve the system of equations
    f = 1/(2*Fs);
    Do = 1/(1-f*a2);
    Dx = 1/(1-f*b2);
    if nonlin == "true"
        tanh_xo = tanh(nonlin_drive*xo);

        yx =(sx + f*b1*yi + f*b3*Do*so + f*b4*(sd+(1/f)*d1*(tanh_xo - xo*(1-tanh_xo^2))) + b4*d1*(1-tanh_xo^2)*Do*so);
        yx = yx/(1-(b2*f+f*f*b3*Do*a1 + b4*f*d1*(1-tanh_xo^2)*Do*a1 + b4*d2));
        yo =(so+f*a1*yx)*Do;
        yd = (sd+(1/f)*d1*(tanh_xo - xo*(1-tanh_xo^2))) + (1/f)*(d1*((1-tanh_xo^2))*yo+d2*yx);

        sx = sx + 2*f*(b1*yi + b2*yx + b3*yo +b4*yd);
        so = so + 2*f*(a1*yx + a2*yo);
        sd = -(sd+(2/f)*d1*(tanh_xo - xo*(1-tanh_xo^2))) - (2/f)*(d1*(1-tanh_xo^2)*yo + d2*yx);
        xo=yo;
    else

        yx =(sx + f*b1*yi + f*b3*Do*so + f*b4*sd + b4*d1*Do*so)*Dx;
        yx=yx/(1-Dx*(f*f*b3*Do*a1 + b4*f*d1*Do*a1 + b4*d2));
        yo =(so+f*a1*yx)*Do;
        yd = sd + (1/f)*(d1*yo+d2*yx) ;

        sx = sx + 2*f*(b1*yi + b2*yx + b3*yo +b4*yd);

        so = so + 2*f*(a1*yx + a2*yo);

        sd = -sd - (2/f)*(d1*yo + d2*yx);  
        
    end;


      output(i) = yo;
end 
%scale output incase of clipping
output = resample(output,1,OS);
output = output/max(output)*.3;


figure()
plot(t,If_v)
grid
title(output_filename + " I\_f")
xlabel('Time (ms)');
ylabel('Vactrol Current (A)');
saveas(gcf,convertStringsToChars("RESULTS\"+ output_filename + " I_f"),'jpeg');


figure()
plot(t,Rf)
grid
title(output_filename + " R\_f")
if save
    saveas(gcf,convertStringsToChars("RESULTS\"+ output_filename + " R_f"),'jpeg');
end

xlabel('Time (s)');
ylabel('Rf(ohms)');
figure()
step=ceil(20*Fs/1000);  %  # one spectral slice every 20 ms
window=ceil(100*Fs/1000);% # 100 ms data window
specgram(output, 2^nextpow2(window), Fs/OS, window, window-step);
output_filename = strcat('RESULTS/',output_filename, '.wav');

title(output_filename);


sound(output,Fs/OS);
if save
    saveas(gcf,convertStringsToChars("RESULTS\"+ output_filename + " SPEC"),'jpeg');
    audiowrite(output_filename,output,Fs/OS);
end
