% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% Linerized model of Di meglio model. Function version.
%
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% Written by:  Knut Åge Meland
% Date:         5.11.2010
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%
function [A,B,C,D]= linmodfunk(x,y0,u,par)

m_G1 = x(1); 
m_G2 = x(2); 
m_L2 = x(3); 


z = u(1);
w_G_in = u(2);
w_L_in = u(3);

%%
P1 = m_G1*par.R*par.T1/(par.M_G*par.Veb);

dP1__dm_G1 = par.R*par.T1/(par.M_G*par.Veb);

%%
V_G2 = par.V2 - (m_L2+par.m_lstill)/par.rho_L;

dV_G2__dm_L2 = -1/par.rho_L;
%%
P2_t = m_G2*par.R*par.T2/(par.M_G*V_G2);
%P2_t = max(P2_t,par.P0);

dP2_t__dm_G2 = par.R*par.T2/(par.M_G*V_G2);
dP2_t__dm_L2 = -m_G2*par.R*par.T2*dV_G2__dm_L2/(par.M_G*V_G2^2);
%%
P2_b = P2_t + par.g*sin(par.theta2)*(m_L2+par.m_lstill)/par.A2;

dP2_b__dm_G2 = dP2_t__dm_G2;
dP2_b__dm_L2 = dP2_t__dm_L2 + par.g*sin(par.theta2)/par.A2;
%%
w_G1 = par.C_g*max(0,P1-P2_b);

dw_G1__dm_G1 = par.C_g*dP1__dm_G1;
dw_G1__dm_G2 = -par.C_g * dP2_b__dm_G2;
dw_G1__dm_L2 = -par.C_g * dP2_b__dm_L2;
%%
w_out = par.C_c*z*sqrt(max(0,P2_t-par.P0));

dw_out__dm_G1 = 0;
dw_out__dm_G2 = par.C_c*z*dP2_t__dm_G2 / (2* sqrt(P2_t-par.P0) );
dw_out__dm_L2 = par.C_c*z*dP2_t__dm_L2 / (2* sqrt(P2_t-par.P0) );

dw_out__dz = par.C_c*sqrt(max(0,P2_t-par.P0));
%%
w_L_out = (m_L2/(m_L2+m_G2))*w_out;

dw_L_out__dm_G1 = 0;
dw_L_out__dm_G2 = (m_L2/(m_L2+m_G2))*dw_out__dm_G2 + ...
    ( (  - m_L2) / (m_L2+m_G2)^2) * w_out ; 
dw_L_out__dm_L2 = (m_L2/(m_L2+m_G2))*dw_out__dm_L2 + ...
    ( ( (m_L2+m_G2)- m_L2) / (m_L2+m_G2)^2) * w_out ;

dw_L_out__dz = (m_L2/(m_L2+m_G2))*dw_out__dz;
%%
w_G_out = (m_G2/(m_L2+m_G2))*w_out;

dw_G_out__dm_G1 = 0;
dw_G_out__dm_G2 = (m_G2/(m_L2+m_G2))*dw_out__dm_G2 + ...
    ( ( (m_L2+m_G2) - m_G2) / (m_L2+m_G2)^2) * w_out ; 
dw_G_out__dm_L2 = (m_G2/(m_L2+m_G2))*dw_out__dm_L2 + ...
    ( ( - m_G2) / (m_L2+m_G2)^2) * w_out ;

dw_G_out__dz = (m_G2/(m_L2+m_G2))*dw_out__dz;

%%
dx1 = (1 - par.ep)*w_G_in - w_G1;
dx2 = par.ep*w_G_in + w_G1 - w_G_out;
dx3 = w_L_in - w_L_out;


%% A,B,C and D:

%A:
dx1__dm_G1 = -dw_G1__dm_G1;
dx1__dm_G2 = -dw_G1__dm_G2;
dx1__dm_L2 = -dw_G1__dm_L2;

dx2__dm_G1 = dw_G1__dm_G1 -dw_G_out__dm_G1;
dx2__dm_G2 = dw_G1__dm_G2 -dw_G_out__dm_G2;
dx2__dm_L2 = dw_G1__dm_L2 -dw_G_out__dm_L2;

dx3__dm_G1 = -dw_L_out__dm_G1;
dx3__dm_G2 = -dw_L_out__dm_G2;
dx3__dm_L2 = -dw_L_out__dm_L2;


%B: 
dx1__dz = 0;
dx1__dw_G_in = (1 - par.ep);
dx1__dw_L_in = 0; 

dx2__dz = -dw_G_out__dz;
dx2__dw_G_in= par.ep;
dx2__dw_L_in = 0;

dx3__dz = -dw_L_out__dz;
dx3__dw_G_in= 0;
dx3__dw_L_in = 1;

%%
A = [dx1__dm_G1, dx1__dm_G2, dx1__dm_L2 ; 
     dx2__dm_G1, dx2__dm_G2, dx2__dm_L2 ;
     dx3__dm_G1, dx3__dm_G2, dx3__dm_L2 ] ;

B = [dx1__dz, dx1__dw_G_in, dx1__dw_L_in ; 
     dx2__dz, dx2__dw_G_in, dx2__dw_L_in ;
     dx3__dz, dx3__dw_G_in, dx3__dw_L_in ];
 
C = 1e-5 .* [dP1__dm_G1, 0,             0 ; 
             0,          dP2_t__dm_G2,  dP2_t__dm_L2 ];
C(3,:) = [dw_out__dm_G1, dw_out__dm_G2, dw_out__dm_L2];

D = zeros(3,3);
D(3,1) = dw_out__dz;


