% A function that will evaluate rdot for the Type 1+2 model in a general
% tree with the Lambert correction factor off, as well as having different
% constants based on the Horsfield order of the airways. This function uses
% connectivity matrices to be able to generalise the equations.

% The function takes as input a vector of the radii, and the vector
% conataining the continuation parameters. It also takes the connectivity
% matrices of the tree.

% DATE: 28/02/18
% AUTHOR: Austin Ibarra
% VERSION: 3.2

% NOTE: - This one is for any arbitrary sized symmetric tree

% == LOG CHANGE == %

% v2.0: - This version does not have C or order as inputs to the function.
%       Rather, "order" has been made a global variable in
%       contType1p2_Xbranch_v1d0.m, so can just calculate C as a persistent
%       variable inside this function.
%
%       The reason for this change is to keep the function input in the
%       same format required for COCO to use.

% v3.0: - This change is simply having epsilon = exp(...), so we can plot
%       log(eps) vs kappa

% v3.1: - Change in an error from "elseif i == N" to elseif "i == numOrd1"

% v3.2: - Removed "order" as a global variable, and instead just had it as
%       a variable that is passed through the function.

function [RRDOT,UVDOT,VVDOT,VDSDOT,out,pbot,p,q, mu] = Type1p2_Connect_GEN_v3d0(r,order,parm,t,uv,vv,vds)
%% Construct the constant array as a persistent variable

% So that repeated calls to this function doesn't result in repeated
% calculations of C
persistent C   

if isempty(C)
    fprintf('Contrusting the structure array of constants...\n');
    C = createConstSymm(order);
    fprintf('Finished.\n');
end

%% Parameters for MoC model
rho=2;
%% Define the constants
f = 0.33;
A = 5e5; % Used to be 5e5;
Pref = 10+ 1*sin(2*pi*0.33*t);
% Rref = 0.2792;
Rref=0.4140;

ptop = Pref;

kappa = parm(1,:);
% epsilon = 10^(parm(2,:));  % A parameter that controls sliding between Type II and Type I + II
epsilon=0;
qhat = -50;

N = 2^order - 1; % number of branches for symmetric trees
NT = (N-1)/2; % number of non-terminating branches
numOrd1 = (N+1)/2;

%% Setting up BG parameters
E = 25;
h1 = 0.9732;
rho1 = 1.225;
for i=1:N
    air_ord = C.order(i);
    CC(i) = (2*pi*C.L(air_ord).*r(i).^3)/(E*h1);
    I(i) = rho1*C.L(air_ord)/(pi.*r(i).^2);
    R(i) = C.alpha(air_ord).*r(i).^(-4);
    Rv(i) = 0.001/CC(i);
end




%% Find expressions for the pressure and flows

Ci = C.Ciplus - C.Ciminus1 - C.Ciminus2;

Dalphar = diag(C.alpha.^(-1).*r.^4);
W = Ci*Dalphar*(C.Cjplus - C.Cjminus);

lambda = dot(C.alpha.^(-1).*r.^4, C.vbot);

temp = (W\(Ci*Dalphar*(C.vbot*C.vbot')*Dalphar*C.Cjplus))/lambda; %optimising code
Lambda = eye(size(temp)) - temp;
p = Lambda\(W\(Ci*Dalphar*(-ptop*C.vtop - 1/lambda*qhat*C.vbot))); %optimising code


% temp = (1/lambda)*(W\(Ci*Dalphar*(C.vbot*C.vbot')*Dalphar*C.Cjplus));
% Lambda = eye(size(temp)) - temp;
% p = (Lambda)\(W\(Ci*Dalphar*(-ptop*C.vtop - 1/lambda*qhat*C.vbot)));

% temp = (1/lambda)*inv(W)*(Ci*Dalphar*(C.vbot*C.vbot')*Dalphar*C.Cjplus);
% Lambda = eye(size(temp)) - temp;
% p = inv(Lambda)*(inv(W)*(Ci*Dalphar*(-ptop*C.vtop - 1/lambda*qhat*C.vbot)));

% pbot = (dot(C.alpha.^(-1).*r.^4.*(C.Cjplus*p),C.vbot) - qhat)/dot(C.alpha.^(-1).*r.^4, C.vbot);

%% Setting up Anafi-Wilson type
pa_bar = E/sqrt(E.^2 + (2*pi*0.33*t*C.alpha.*r.^(-4)).^2);
alph = atan(2*pi*0.33*t*C.alpha.*r.^(-4)/E);
pbot = 10+pa_bar*sin(2*pi*0.33*t-alph);

for a = numOrd1+1:N
    uv1(a) = (vv(a)-vv(2*(a-numOrd1))-vv(2*(a-numOrd1)-1))/CC(a);
    u(a) = uv1(a) +Rv(a)*((2*(a-numOrd1))-vv(2*(a-numOrd1)-1));
end

for b = 1:numOrd1
    uv1(b) = (vv(b)-vds(b))/CC(b);
    u(b) = uv1(b) + Rv(b)*(vv(b)-vds(b));
    vds1(b) = (uv1(b)-pbot-(R(b)/2)*vds(b))/(I(b)/2);
end


for d = 1:N-1
    a =((2*d) - 1);
    aa = a(a<N-1); 
    b(d) = 2*d;
    bb = b(b<=N-1);
end

for i = 1: length(aa)
    vv1(aa(i)) = (uv(NT+d) - u(aa(i)) - (R(aa(i))*vv(aa(i)))/2)./(I(aa(i))/2);
    vv1(bb(i)) = (uv(NT+d) - u(bb(i)) - (R(bb(i))*vv(bb(i)))/2)./(I(bb(i))/2);
end
vv1(N) = (Pref - u(N) - R(N)*vv(N))/I(N);

q = C.alpha.^(-1).*r.^4.*((C.Cjplus - C.Cjminus)*p + ptop*C.vtop - pbot*C.vbot);

gammajplus = C.Cjplus*p + ptop*C.vtop;
gammajminus = C.Cjminus*p + pbot*C.vbot;
deltap = gammajplus - gammajminus;

T = C.T;    % The transformation matrix that will map mu_hat to mu

%% Determine what mu is (parenchymal shear modulus)

mu = zeros(N,1);

for i = 1:numOrd1  % Wanting to loop through only the order 1 airways
        
    % Add the Type 1 coupling by putting in the dependence on the
    % neighbours. 
    if i == 1
        mu(i) = abs(A/3*(deltap(i)*r(i)^4 + epsilon*(deltap(numOrd1)*r(numOrd1)^4 + deltap(i+1)*r(i+1)^4)))/2;
    elseif i == numOrd1
        mu(i) = abs(A/3*(deltap(i)*r(i)^4 + epsilon*(deltap(i-1)*r(i-1)^4 + deltap(1)*r(1)^4)))/2;
    else
        mu(i) = abs(A/3*(deltap(i)*r(i)^4 + epsilon*(deltap(i-1)*r(i-1)^4 + deltap(i+1)*r(i+1)^4)))/2;
    end

end

% Use the transformation matrix, T, to create the full mu vector
mu = T*mu;

%% Determine what tau is (parenchymal tethering pressure)
tau = 2*mu.*(((Rref - r)/Rref) + 1.5*((Rref - r)/Rref).^2);

%% Determine what Ptm is (transmural pressure)


pmid = 0.5*(gammajplus + gammajminus);

% Ptm = pmid - kappa*Rref./r + tau;

%% Determine what R is (airway radius)
R = zeros(N,1);
RDOT=zeros(N,1);
% F=zeros(4*M,N);

% n11=RR(1:M);
% n22=RR( (M+1):(2*M));
% n33=RR( ((2*M+1)):(3*M));
% n44=RR( ((3*M+1)):(4*M));

% force=zeros(N,1);
for i=1:N
    air_ord = C.order(i);  % Determine the order of the current airway
    %% Initial condition required for Method of Characteristics 

% X=X0+((gamma/(2*pi*Rref))*(r(end)-rinitial(end)));%update X as radii changes
% 
% % Binding rate functions gx,gpx and fpx
% [gx, gpx, fpx] = bindingfuncs(X);
% 
% force=(kappa).*trapz(X,X.'.*(n33+n44));

Ptm(i) = pmid(i) - (kappa*Rref/r(i)) + tau(i);

    if Ptm(i) <= 0
        R(i) = sqrt((C.Ri(air_ord).^2)*(1 - Ptm(i)./C.P1(air_ord)).^(-C.n1(air_ord)));
    elseif Ptm(i) >= 0
        R(i) = sqrt(C.rimax(air_ord).^2 - (C.rimax(air_ord).^2 - C.Ri(air_ord).^2)*(1 - Ptm(i)/C.P2(air_ord)).^(-C.n2(air_ord)));
    end

%     F=[-1*k1'.*n11+k2*n22+gx'.*n44;k1'.*n11-((k2+fpx)'.*n22)+gpx'.*n33;fpx'.*n22-((k5+gpx)'.*n33)+k6'.*n44;k5*n33-((k6+gx)'.*n44)];
   RDOT=rho*((R)-r);
end
% Phi=R;
% RDOT=rho*((R)-r);

RRDOT=RDOT;
UVDOT = [uv1]';
size(UVDOT)
VVDOT = [vv1]';
size(VVDOT)
VDSDOT = [vds1]';
size(VDSDOT)
%% Final output
% K=F;
 
%% Output containing all of the variables

% out.K = K;
% out.R = R;
out.Ptm = Ptm;
out.tau = tau;
out.mu = mu;
out.q = q;
out.p = p;
out.pbot = pbot;
out.deltap = deltap;
out.W = W;
out.lambda = lambda;
out.Lambda = Lambda;
out.gammajplus = gammajplus;
out.gammajminus = gammajminus;
out.flow=q;

end