function [results] = ComputeSpringDamperRenderingStabilityAndInstrumentationErrorLimits_ImpedanceType(...
                       m,b,c,B,delta,T,f_0,n,delay)
% ComputeSpringDamperRenderingStabilityAndQuantizationErrorLimits(m,b,c,B,delta,T,f_0,n,delay)
% PURPOSE: This file computes the stability and instrumentation error 
%          limits for virtual spring and damper rendering on an 
%          impedance-type haptic display. 
% AUTHOR: Nick Colonnese
%         Oct 10, 2017
%         Oculus Research 
% INPUTS: 12 scalars, 
%         m      robot mass (kg) 
%         b      robot damping (Ns/m)
%         c      robot friction (N) 
%         B      virtual damping (Ns/m) 
%         delta  robot quantization interval (m)
%         T      sample rate (s)
%         f_0    low-pass filter cut-off frequency (Hz) 
%         n      low-pass filter order  
%         delay  time delay (in units of the sample time)
%         m_h    human mass (kg) 
%         b_h    human damping (Ns/m)
%         k_h    human stiffness (N/m) 
% OUTPUTS: 1x1 struct titled "results" with 6 elements,
%         0) K_sampledDataPassive   sampled data passivity limit        (N/m)
%         1) K_uncoupledStable      uncoupled stiffness stability limit (N/m)
%         2) K_coupledStable        coupled stiffness stability limit   (N/m)
%         3) K_quantErrorPassive    quantization error passivity limit  (N/m)
%         4) K_malTouchLimitCycles  malicious touch limit cycles        (N/m)
%         5) K_uncoupledLimitCycles uncoupled limit cycles              (N/m)

%%
% Implementation 
%-------------------------------------------------------------------------

%% Construct Transfer Functions 
% for both the uncoupled (no environment), and coupled systems, 
%  convert the systems to entirely discrete through a zero-order hold 
%  equivalent on all the continuous dynamics 
G_uncoupled = tf(1,[(m + 0) (b + 0) 0]);
G_uncoupled_d = c2d(G_uncoupled,T,'zoh');  

G_coupled = tf(1,[(m + m_h) (b + b_h) k_h]);
G_coupled_d = c2d(G_coupled,T,'zoh');  

% create control law for spring damper
%  back difference 
D_d = tf([1 -1],[T 0],T); 
%  single order dc gain low-pass
H_d = tf([(1 - exp(-f_0*2*pi*T)) 0],[1 -(exp(-f_0*2*pi*T))],T);

% delay tf
delay_d = tf(1,[1 0],T);

% loop polynomials (coefficient is K)
L_uncoupled = (G_uncoupled_d*delay_d^delay)/(1 + G_uncoupled_d*D_d*H_d^n*B*delay_d^delay); 
L_coupled = (G_coupled_d*delay_d^delay)/(1 + G_coupled_d*D_d*H_d^n*B*delay_d^delay); 

%% Find Stiffness Limit for Uncoupled System 
% 1) call all margin 
marginInfo = allmargin(L_uncoupled); 

uncoupledGainMargin = -1; % if no gain margins return -1
% 2) populate possible gain margins and sort 
if( isempty(marginInfo.GainMargin) ) % if no gain margins leave at -1 
    % do nothing 
else
    uncoupledGainMargins = zeros(length(marginInfo.GainMargin),1); 
    for i = 1:length(marginInfo.GainMargin); 
        uncoupledGainMargins(i) = marginInfo.GainMargin(i); 
    end
    uncoupledGainMargins = sort(uncoupledGainMargins); 
end

% 3) pick the gain margin that is predicted to be correct
minGainMargin = 0; 
maxGainMargin = (b + B)/(T/2 + delay*T - T/5); % max from unfilterd velocity
                                               % the T/5 is for numerics 

for i = 1:length(marginInfo.GainMargin); 
    if( (uncoupledGainMargins(i) > minGainMargin) &&...
        (uncoupledGainMargins(i) < maxGainMargin) )
    uncoupledGainMargin = uncoupledGainMargins(i); 
    end
end   

K_uncoupled = uncoupledGainMargin; 

%% Find Stiffness Limit for Coupled System 
% 1) call all margin 
marginInfo = allmargin(L_coupled); 

coupledGainMargin = -1; % if no gain margins return -1
% 2) populate possible gain margins and sort 
if( isempty(marginInfo.GainMargin) ) % if no gain margins leave at -1 
    % do nothing 
else
    coupledGainMargins = zeros(length(marginInfo.GainMargin),1); 
    for i = 1:length(marginInfo.GainMargin); 
        coupledGainMargins(i) = marginInfo.GainMargin(i); 
    end
    coupledGainMargins = sort(coupledGainMargins); 
end

% 3) pick the gain margin that is predicted to be correct
minGainMargin = 0; 
maxGainMargin = (b + B)/(T/2 + delay*T - T/5); % max from unfilterd velocity
                                               % the T/5 is for numerics 

for i = 1:length(marginInfo.GainMargin); 
    if( (coupledGainMargins(i) > minGainMargin) &&...
        (coupledGainMargins(i) < maxGainMargin) )
    coupledGainMargin = coupledGainMargins(i); 
    end
end   

K_coupled = coupledGainMargin; 

%% Find Maximum Stiffness for Quantization Error Passivity
% find l1 norm of damping control law 
y = impulse(D_d*H_d^n);
l1_norm = sum(abs(y)); 
% find K limit in N/m
K_quantizationErrorPassive = (c/delta) - B*l1_norm; 

%% Find Maximum Stiffness for Malicious Touch and Uncoupled Limit Cycles
[y ~] = impulse(D_d*H_d^n);
BAF = 0; 
for i = 1:length(y)
    if( (mod(i,2) == 1) ) %odd
        BAF = BAF + y(i);
    else                  %even
        BAF = BAF - y(i); 
    end
end

K_maliciousTouchLimitCycles = (c/delta) - B*BAF; 
K_uncoupledLimitCycles = (c/delta) + (2*b/T) - B*BAF;

%% Find Maximum Stiffness for Sampled Data Passivity
% NOTE: The code to generate the maximum stiffness for sampled data 
%       passivity is much more computationally intensive than finding the 
%       uncoupled or coupled stability limits, or quantization error
%       limits. Because the positive virtual damping damping coefficient is
%       reasonaly approximated by a look up table corresponding to the 
%       ratio of the cut-off frequency to the low-pass filter, that is 
%       what we use here. 
%       If you wish to get the exact value, change 
%       approximatePositiveVirtualDampingCoefficient to "false" 
approximatePositiveVirtualDampingCoefficient = true; 

% initialize value to zero 
K_sampledDataPassive = 0;  %#ok<NASGU>

% find value using approximate method 
if(approximatePositiveVirtualDampingCoefficient)
    if( (f_0*T < 0.2) )
        positiveVirtualDampingCoefficient = 0.25; 
    elseif( (f_0*T < 0.4) )
         positiveVirtualDampingCoefficient = 0.50; 
    elseif( (f_0*T < 0.6) )
         positiveVirtualDampingCoefficient = 0.75; 
    elseif( (f_0*T < 0.8) )
         positiveVirtualDampingCoefficient = 0.9; 
    else
         positiveVirtualDampingCoefficient = 1; 
    end
    K_sampledDataPassive = (b - B*positiveVirtualDampingCoefficient)/(T/2 + delay*T);
else % find value using exact method 
    %set up symbolic variables 
    T = sym('T','positive');
    K = sym('K','positive');
    B = sym('B','positive');
    w = sym('w','positive');
    w_0 = sym('w_0','positive');
    
    %create expression
    z = exp(1j*w*T); 
    D = ((z - 1)/(T*z));
    H_filt = (z*(1 - exp(-w_0*T))/(z - (exp(-w_0*T))));
    delay = (1/z); 
    H = (K + B*D*H_filt^n)*delay^d; 
    expr = (T/2)*(1/(1 - cos(w*T)))*real( (1 - exp(-1j*w*T))*H );

    %subsitute and find max and mins for the damping coefficient 
    T_value = (1/f); 
    w_0_value = 2*pi*f_0; 
    y = subs(expr,[K B, T, w_0],[1 1 T_value w_0_value]);
    ht = matlabFunction(-y);
    argmax = fminbnd(ht,0,maxRange);
    B_coeffMax = -real(ht(argmax)) - ((1/2)*(1/f) - d*(1/f));
    positiveVirtualDampingCoefficient = B_coeffMax; 
    
    % find value 
    K_sampledDataPassive = (b - B*positiveVirtualDampingCoefficient)/(T/2 + delay*T);
end % end to if(approximatePositiveVirtualDampingCoefficient)

%% Clip All Maximum K's At Zero If Negative 
if(K_uncoupled < 0)
    K_uncoupled = 0; 
end

if(K_coupled < 0)
    K_coupled = 0; 
end

if(K_quantizationErrorPassive < 0)
    K_quantizationErrorPassive = 0; 
end

if(K_maliciousTouchLimitCycles < 0)
    K_maliciousTouchLimitCycles = 0; 
end

if(K_uncoupledLimitCycles < 0)
    K_uncoupledLimitCycles = 0; 
end

%% Put All Results In Struct 
results.K_sampledDataPassive = K_sampledDataPassive; 
results.K_uncoupled = K_uncoupled;
results.K_coupled = K_coupled;
results.K_quantizationErrorPassive = K_quantizationErrorPassive; 
results.K_maliciousTouchLimitCycles = K_maliciousTouchLimitCycles; 
results.K_uncoupledLimitCycles = K_uncoupledLimitCycles; 

