function [x, sc, d] = initialize(Ph, Pl, DTihx, Tgco_out, Qgco, DTgco_air, u, d)
global Tk
Tgco_air = u(1);
Tvap_air = u(2);

T_to_comp = Tsat(Pl)+DTihx;
Vtmp = 1;
N_to_comp = N_given_TVP([T_to_comp;Vtmp;Pl], 'v');
x_to_comp = [T_to_comp; Vtmp; N_to_comp];
s_to_comp = s_given_TVN(x_to_comp);
h_to_comp = h_given_TVN(x_to_comp);
cp_to_comp = heat_capacity(x_to_comp);
Tout_vap = T_to_comp-DTihx;
Ntmp = N_given_TVP([Tout_vap; Vtmp; Pl], 'v');
hout_vap = h_given_TVN([Tout_vap; Vtmp; Ntmp]);

% Compressor
ucom = [h_to_comp; s_to_comp; Ph; Pl; cp_to_comp; T_to_comp];
[xcom, sc.com] = initial_compressor(ucom, d);
h_com = xcom(2);
Ti_com = xcom(3);
% Gas cooler
ugco = [h_com; Ph; Ti_com; Tgco_out; Qgco; Tgco_air; DTgco_air];
[xgco, sc.gco, n, hgco] = initial_GasCooler(ugco, d);
% Internal heat exchanger
Tout_gco = xgco(2*d.ngco);
uihx = [Tout_gco; Ph; Pl; n; T_to_comp; hout_vap];
[xihx, sc.ihx, hihx1, hihx2, rho_to_valve] = initial_internalHX(uihx, d);
% Valve
uval = [hihx1; Ph; Pl; n; rho_to_valve];
[xval, sc.val, d, hval] = initial_valve(uval, d);
% Evaporator
uvap = [hval; Pl; n; Tout_vap; Tvap_air];
[xvap, sc.vap] = initial_evaporator(uvap, d);

% Collect state vector
sc.x = [sc.com.x; sc.gco.x; sc.val.x; sc.vap.x; sc.ihx.x];
sc.res = [sc.com.res; sc.gco.res; sc.val.res; sc.vap.res;sc.ihx.res];
x = [xcom; xgco; xval; xvap; xihx];

function [xihx, sc, hihx_1, hihx_2, rho_to_valve] = initial_internalHX(u, d)
disp('Initialize internal heat exchanger')
Tinlet_1 = u(1)
P_1 = u(2);
P_2 = u(3);
n = u(4);
Toutlet_2 = u(5);
hinlet_2 = u(6);
Ninlet_1 = N_given_TVP([Tinlet_1; d.Vihx1/d.nihx; P_1], 'c');
Noutlet_2 = N_given_TVP([Toutlet_2; d.Vihx2/d.nihx; P_2], 'v');
hinlet_1 = h_given_TVN([Tinlet_1; d.Vihx1/d.nihx; Ninlet_1]);
houtlet_2 = h_given_TVN([Toutlet_2; d.Vihx2/d.nihx; Noutlet_2]);
A = 0.1;
htmp2in=0;
h_tmp=hinlet_2;
while abs((hinlet_2-htmp2in)/hinlet_2)>1e-6
A = A-abs(hinlet_2-h_tmp)*1e-5;
  x0 = [Tinlet_1; Ninlet_1; Toutlet_2; Noutlet_2; houtlet_2];
  sc0.x = x0;
  sc0.res = abs([100;
                 100;
                 houtlet_2;
                 P_2;
                 P_1]);
  u = [d.Vihx1/d.nihx; d.Vihx2/d.nihx; n; hinlet_1; houtlet_2; P_1; P_2; A/d.nihx; d.Uihx];
  x=[];T1=[];T2=[];N1=[];N2=[];
  for i = 1:d.nihx
    x0 = x0./sc0.x;
    aux.d = d;
    aux.u = u;
    aux.sc = sc0;
    options = optimset('display', 'off');
    [x, fval] = fsolve(@ihx_section, x0, options, aux);
    x = x.*sc0.x;
    T1(i, 1)=x(1);
    N1(i, 1)=x(2);
    T2(i, 1)=x(3);
    N2(i, 1)=x(4);
    u(4) = h_given_TVN([T1(end); d.Vihx1/d.nihx; N1(end)]);
    u(5) = x(end); 
    x0=x;
  end
  htmp2in = x(end);
  h_tmp=htmp2in;
end
hihx_1 = u(4);
hihx_2 = houtlet_2;
u = [hinlet_1; P_1; hinlet_2; P_2; n];
Q = d.Uihx*A/d.nihx*(T1-T2);
x = [T1; N1; Q; T2; N2; A];
sc = scale(x, d, 'ihx');
aux.d = d;
aux.sc = sc;
aux.u = u;
res = InternalHX(x./sc.x, aux);
xihx = x;
rho_to_valve = N1(end)/(d.Vihx1/d.nihx);

function [xcom, sc] = initial_compressor(u, d)
disp('Initializing compressor')
h_in = u(1);
s_in = u(2);
Ph = u(3);
Pl = u(4);
cp = u(5);
T_in = u(6);

Ti = T_in*(Ph/Pl)^(d.R/cp);
Ni = d.Ni;
Vi = V_given_TPN([Ti; Ph; Ni], 'v');
hi = h_given_TVN([Ti; Vi; Ni]);
h(1) = (hi-h_in)/d.eta+h_in;
ws = h(1)-h_in;
x0 = [ws; h(1); Ti; Vi];
sc0 = scale(x0, d, 'com');
x0 = x0./sc0.x;
options = optimset('fsolve');
%options = optimset(options, 'TolFun', 1e-15);
aux.d = d;
aux.u = u;
aux.sc = sc0;
[x, fval] = fsolve(@compressor, x0, options, aux);
xcom = x.*sc0.x;
sc = scale(xcom, d, 'com');

function [xgco, sc, n, hgco] = initial_GasCooler(u, d)
disp('Initializing gas cooler')
hinlet = u(1);
P = u(2);
Tinlet = u(3);
Toutlet = u(4);
Qgco = u(5);
Tair_inlet = u(6);
DTair = u(7);
Tair_outlet = Tair_inlet+DTair;
%T =  [Tinlet:(T_out-Tinlet)/(d.ngco-1):T_out]';
DTm = -((Tinlet-Tair_outlet)+(Toutlet-Tair_inlet))/log((Tinlet-Tair_outlet)/(Toutlet-Tair_inlet));
Area = Qgco/(d.Ugco*DTm);
Nend = N_given_TVP([Toutlet; d.Vgco/d.ngco; P], 'c');
h_out = h_given_TVN([Toutlet; d.Vgco/d.ngco; Nend]);
n = Qgco/(h_out-hinlet);
nair = -Qgco/(d.Cp_air*DTair);
T_s = 0;
Ts = Toutlet;
while abs(Toutlet-T_s(end))/Toutlet>1e-6
  Area = Area+(Ts(end)-Toutlet)*5e-2;
  Ns=[];Ts=[];Tair=[];
  T_in = Tinlet;
  h_in = hinlet;
  Tair_out = Tair_outlet;
  for i = 1:d.ngco
    N = N_given_TVP([T_in; d.Vgco/d.ngco; P], 'c');
    x0 = [T_in; N];
    sc0.x = x0;
    sc0.res = [Qgco; P];
    options = optimset('Display', 'off');
    u = [d.Vgco/d.ngco; n; h_in; Tair_out; P; Area/d.ngco; d.Ugco];
    aux.u = u;
    aux.sc = sc0;
    aux.d = d;
    x = fsolve(@control_volume, x0./sc0.x, options, aux);
    x = x.*sc0.x;
    Ts = [Ts; x(1)];
    Tair = [Tair; Tair_out];
    Tair_out = d.Ugco*Area/d.ngco*(Tair_out-x(1))/(nair*d.Cp_air)+Tair_out;
    Ns = [Ns; x(2)];
    h_in = h_given_TVN([x(1); d.Vgco/d.ngco; x(2)]);
    T_in = x(1);
  end
  T_s = Ts(end);
end
% Bruker TOMLAB for å finne alle tilstander for å oppfylle kjølebehovet og
% temperaturen ut.
Q = d.Ugco*Area/d.ngco*(Tair-Ts);
x0 = [Ns; Ts; Q; Tair; nair; P; Area];
u = [hinlet; n; ;Tair_inlet];

sc0 = scale(x0, d, 'gco');
x0 = x0./sc0.x;
in.d = d;
in.u = u;
in.sc = sc0;
res = GasCooler(x0, in);
f = 'objfun';
g = [];
H = [];
HessPattern = [];
x_L = [zeros(d.ngco,1); zeros(d.ngco,1); -inf*ones(d.ngco,1); zeros(d.ngco, 1); 0; 0; 0]./sc0.x;
x_U = [inf*ones(d.ngco,1); inf*ones(d.ngco,1); inf*ones(d.ngco,1); inf*ones(d.ngco, 1); inf; inf; inf]./sc0.x;
Name = 'Initialization of gas cooler';
pSepFunc = [];
fLowBnd = [];
b_L = [P; Toutlet; Qgco; Tair_outlet; nair];
b_U = [P; Toutlet; Qgco; Tair_outlet; nair];
A = [0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  1  0; % P
     0  0  0  0  0  0  0  0  0  0  0  1  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0; % T_out
     0  0  0  0  0  0  0  0  0  0  0  0  1  1  1  1  1  1  0  0  0  0  0  0  0  0  0; % Q
     0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  1  0  0  0  0  0  0  0  0; % Tair_out
     0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  1  0  0; % nair
     ];
SCALER = [];
for i=1:size(A,1)
    SCALER = [SCALER; sc0.x'];
end
A=A.*SCALER;
c = 'GasCooler';
dc = [];
d2c = [];
ConsPattern = [];
c_L = zeros(size(res));
c_U = zeros(size(res));
x_min = [];
x_max = [];
f_opt = [];
x_opt = [];
Prob = ProbDef;
Prob = conAssign(f, g, H, HessPattern, x_L, x_U, Name, x0, pSepFunc, fLowBnd, A, b_L, b_U, c, dc, d2c, ConsPattern, c_L, c_U, x_min, x_max, f_opt, x_opt);
Prob.d=d;
Prob.u=u;
Prob.sc=sc0;
%Prob.optParam.cTol = 1e-15;
%Prob.optParam.bTol = 1e-15;
Result = tomRun('minos', Prob);
disp(Result.ExitText)
x = Result.x_k;
xgco = x.*sc0.x;
hgco = h_given_TVN([xgco(2*d.ngco); d.Vgco/d.ngco; xgco(d.ngco)]);
sc = scale(xgco, d, 'gco');


function [xval, sc, d, hout] = initial_valve(u, d)
disp('Initialize valve')
hin = u(1);
Ph = u(2);
Pl = u(3);
n = u(4);
rho = u(5);
z = 0.5;
options = optimset('fsolve');
d.Cv = fsolve(@valve_size, 1, options, z, rho, n, Ph-Pl, d.MW);
hout = hin;
u = [hin; rho; Pl; Ph];
x = [z; n; hout];
sc = scale(x, d, 'val');
x = x./sc.x;
aux.d = d;
aux.u = u;
aux.sc=sc;
res = valve(x, aux);
if norm(res)<1e-6
  disp('OK')
else
  disp('Not OK')
end
xval = x.*sc.x;

function [x, sc] = initial_evaporator(u, d)
disp('Initialize evaporator')
hinlet = u(1);
P = u(2);
n = u(3);
T = u(4);
Tair = u(5);

hout = h_given_TVN([T; 1; N_given_TVP([T; 1; P], 'v')]);
Q = n*(hout-hinlet);
A = Q/(d.Uvap*(Tair-T));

Vl = 0.5*d.Vvap;
Vv = d.Vvap-Vl;
Nl = N_given_TVP([T; Vl; P], 'l');
Nv = N_given_TVP([T; Vv; P], 'v');
x = [T; Nl; Nv; Vl; Q; A; P];
sc = scale(x, d, 'vap');
u = [hinlet; n; Tair];
in.d = d;
in.u = u;
in.sc = sc;
res = evaporator(x./sc.x, in);

function res = control_volume(x, aux)
sc = aux.sc;
u = aux.u;
x = x.*sc.x;
T = x(1);
N = x(2);
V = u(1);
n = u(2);
hin = u(3);
T2 = u(4);
P = u(5);
A = u(6);
U = u(7);

h = h_given_TVN([T; V; N]);
Atmp = co2_sw([T; V; N]);
Ptmp = -Atmp.g(2);
Q = U*A*(T2-T);
res = [(hin-h)*n+Q;
       P-Ptmp]./sc.res;

function res = ihx_section(x, aux)
sc = aux.sc;
u = aux.u;
V1 = u(1);
V2 = u(2);
n = u(3);
hin1 = u(4);
hout2 = u(5);
P1 = u(6);
P2 = u(7);
A = u(8);
U = u(9);
x = x.*sc.x;
T1 = x(1);
N1 = x(2);
T2 = x(3);
N2 = x(4);
hin2 = x(5);
x1 = [T1; V1; N1];
x2 = [T2; V2; N2];
Q = U*A*(T1-T2);
hout1 = h_given_TVN(x1);

res = [Q+n*(hout1-hin1);
       Q-n*(hout2-hin2);
       hout2-h_given_TVN(x2);
       P2-P_given_TVN(x2);
       P1-P_given_TVN(x1)]./sc.res;

function res = valve_size(Cv, z, rho, n, DP, MW)
res = n-valve_flow(Cv, z, rho, DP, MW);
