% Optimize heat in the room using only qhr (heat in the room)
%
%
function [sol,res] =propt_room_1Q(par,x0,u0,tinit,sol_guess,simtime,horizon)

%  
%   sol_guess=proptnom(par,x0);
%   save sol_guess
% 

 for n=[25 45]
 
toms t  % Free final time
z1=tom('z1');
z2=tom('z2');

% t_f=horizon+tinit;
%Shirinking horizon
t_f=min(horizon+tinit,simtime);
p = tomPhase('p', t, tinit, t_f, n,[],'gauss');

setPhase(p);
 
tomStates Tr Tf mr co2 qair 
tomControls qhr qhf  

qhr_max=10;
qhf_max=qhr_max;
Tf_max=27+273;
Tr_max=Tf_max;

Tfmin=0+273;
Trmin=17+273;
co2max=par.co2max*1.0;


%  qhf=0*t;
% Box constraints
 cbox = {0 <=collocate(qhf)<=qhf_max,0 <=collocate(qhr)<=qhr_max , 0<=mcollocate(qair)<=0.05, Tfmin<=mcollocate(Tf)<=Tf_max,  (Trmin-z2)<=mcollocate(Tr)<=Tr_max+z2,-z1<=mcollocate(co2)<=co2max+z1,mcollocate(z1>=0),mcollocate(z2>=0)};
%   cbox = {0 <=collocate(qhf)<=qhf_max,0 <=collocate(qhr)<=qhr_max , 0<=mcollocate(qair)<=0.05,0<=mcollocate(co2)<=co2max+softc,0<=mcollocate(softc)};
%  cbox={};

% Boundary constraints
% cbnd = {initial({Tf == x0(1); Tr==x0(2) ; mr==x0(3); co2==x0(4)});};
cbnd = {initial({ qhf==u0(1);qhr==u0(2);qair==u0(3); Tf == x0(1); Tr==x0(2) ; mr==x0(3); co2==x0(4)});};

if n==25
% Initial guess
initialguess = {icollocate(qhf ==sol_guess.qhf),icollocate(qhr ==sol_guess.qhr),...
    icollocate(qair ==sol_guess.qair), collocate(mr==sol_guess.mr),collocate(Tr==sol_guess.Tr),...
    collocate(Tf==sol_guess.Tf),collocate(co2==sol_guess.co2),collocate(z1==0),collocate(z2==0)};

else
    initialguess = {icollocate(qhf ==sol.qhf),icollocate(qhr ==sol.qhr), icollocate(qair ==sol.qair), collocate(mr==sol.mr),collocate(Tr==sol.Tr), collocate(Tf==sol.Tf),collocate(co2==sol.co2)};
    
end
% Identify parameters
UAfr = par.UAfr;
UAro = par.UAro;
Cpr = par.Cpr;
Cpf = par.Cpf;
Po = par.Po;
mf = par.mf;
Mr = par.Mr;
R = par.R;
k = par.k;
Vr=par.Vr;
co2rate=par.co2rate;
co2in=par.co2in;

%   To=273+5*sin(2*pi*(t-6*3600)/(24*3600)-pi/2); 
hours=linspace(0,24,24);

 %predicted temperature
  temp=273+[-1.2 -1.1 -2.2 -1.7 -2.9 -3.6 -3.3 -4.2 -3.4 -1.9 -2.3 -3.5 -2.2 0.9 1.1 1.1 1.0 1.4 1.2 1.9 2.9 3.5 3.1 3.3];

 %real temperature
  %min of the day
%   temp=273+[-1.3 -1.2 -2.2 -2.8 -2.9 -3.6 -4.2 -4.5 -4.4 -4.1 -3.5 -3.5 -3.5 -2.0 0.8 0.9 0.6 1.0 0.9  -0.2 1.9 2.9 3.1 2.9];
  %max of the day
%    temp=273+5+[-0.4 -0.5 -1.1 -1.6 -1.5 -2.3 -2.7 -3.4 -2.7 -1.9 -1.6 -1.4 -0.6 0.9 1.1 1.3 1.1 1.5 1.4 2.3 3.0 3.6 3.7 3.5];
  
To=interp1(hours,temp,t/3600,'linear','extrap');
%  To=273+0;
 
 %price: NOK/KWh
  price=2+1*sign(sin(2*pi*(t-8*3600)/(12*3600)+1.*pi/2));
 
%   price=2;
%     To=273;

qout=k*((mr/Mr/Vr)*R*Tr-Po);
co2breath=0.5*co2rate/mr;

%         deltaCo2=-max(0,qout)*co2/mr+max(0,-qout)*co2in/mr
%      deltaCo2=-0.5*(1+tanh(qout/1e4))*co2/mr+0.5*(1-tanh(qout/1e4))*co2in/mr;
%       deltaCo2=-co2/mr*(1+tanh(qout/1e-4))*0.5+0.5*(1-tanh(qout/1e-4))*co2in/mr;
%         deltaCo2=-max(0,qout)*co2/mr+max(0,-qout)*co2in/mr;
% ODEs and path constraints
ceq = collocate({    
%         dot(Tf) == (qhf/(mf*Cpf))-(UAfr/(mf*Cpf))*(Tf-Tr)
        dot(Tf)== qhf/(mf*Cpf)-(UAfr/(mf*Cpf))*(Tf-Tr)
%         dot(Tr) == (qair/mr)*(To-Tr)+(qhr/(mr*Cpr))+(UAfr/(Cpr*mr))*(Tf-Tr)-(UAro/(Cpr*mr))*(Tr-To)
       dot(Tr) == (qair/mr)*(To-Tr)+qhr/mr+(UAfr/(Cpr*mr))*(Tf-Tr)-(UAro/(Cpr*mr))*(Tr-To)
%         dot(mr) == qair-k*((mr/Mr/Vr)*R*Tr-Po);
        dot(mr) == qair-qout

        dot(co2)==qair*(co2in-co2)/mr+co2breath
%          dot(co2)==qair*co2in/mr+co2breath+qout*co2/mr
        
        });


mu=   3.4327e+8;
%  c1=integrate(softc^2);
% c2=mcollocate(Tf-Tfmin);
% c3=mcollocate(co2max-co2);
% mu*c1+0*mu*max(0,c2)+0*mu*max(0,c3)
% +integrate(1e8*dot(qair)^2+dot(qhr)^2)
objective =integrate(price/3600*(qhf+qhr))+ 0.001*integrate(price/3600*(qhf+qhr)^2)+integrate(1e7*dot(qair)^2)+100*integrate(dot(qhf)^2)+100*integrate(dot(qhr)^2)+mu*z1+10*mu*z2;
% objective=0;

options = struct;
% options.PriLevOpt = 1;
% options.Prob.SOL.optPar(30) = 20000;
options.name = 'Room heating';
%   options.solver= 'snopt';
   options.solver= 'npsol';
%  options . scale = 'auto ';
constr = {cbox, cbnd, ceq};


 
 
[solution,res] = ezsolve(objective, constr, initialguess, options);

if (res.ExitFlag~=0)&&(res.ExitFlag~=3)
    display('Trying again')
   solguess=feasibilitycheck(constr,initialguess); 
   [solution,res] = ezsolve(objective, constr, solguess, options);
end

% res
% pause

sol.t = subs(collocate(p,t),solution);
sol.qhf = subs(collocate(p,qhf),solution);
sol.qhr = subs(collocate(p,qhr),solution);
sol.mr = subs(collocate(p,mr),solution);
sol.qair = subs(collocate(p,qair),solution);
sol.Tf=subs(collocate(p,Tf),solution);
sol.Tr=subs(collocate(p,Tr),solution);
sol.co2=subs(collocate(p,co2),solution);
sol.qout=subs(collocate(p,qout),solution);

 end
 
 
%  softc=subs(collocate(p,softc),solution)
 
 J=subs(collocate(p,objective),solution);
J=J; % NOK
price=subs(collocate(p,price),solution);
To=subs(collocate(p,To),solution);
t_f=subs(collocate(p,t_f),solution);

sol.t_f=t_f(end);
sol.price=price;
sol.To=To;
sol.J=J;
% 
% figure(1)
% subplot(311)
% plot(sol.t,sol.Tf,sol.t,ones(size(sol.Tf))*Tf_max,'r',sol.t,ones(size(sol.Tr))*Trmin,'r')
% set(gca,'FontSize',14)
% xlabel('Time,h')
% ylabel('T floor, ^\circ C')
% axis tight
% subplot(312)
% % plot(sol.t,sol.Tr,sol.t,ones(size(sol.Tr))*Trmin,'r',sol.t,ones(size(sol.Tr))*Tr_max,'r')
% plot(sol.t,sol.Tr,sol.t,ones(size(sol.Tr))*Trmin,'r')
% set(gca,'FontSize',14)
% xlabel('Time,h')
% ylabel('T room, ^\circ C')
% 
% subplot(313)
% plot(sol.t,sol.mr)
% set(gca,'FontSize',14)
% xlabel('Time,h')
% ylabel('Mass room, Kg')
% 
% figure(2)
% subplot(311)
% plot(sol.t,sol.qhf)
% set(gca,'FontSize',14)
% xlabel('Time,h')
% ylabel('Q_f, kW')
% 
% subplot(312)
% plot(sol.t,sol.qhr)
% set(gca,'FontSize',14)
% xlabel('Time,h')
% ylabel('Q_r, kW')
% 
% subplot(313)
% plot(sol.t,sol.qair)
% set(gca,'FontSize',14)
% xlabel('Time,h')
% ylabel('q_air, Kg/s')
% 
% figure
% subplot(311)
% plot(sol.t,To)
% strJ=strcat('J=',num2str(J(end)),' NOK in ',num2str(t_f/3600),' h');
% set(gca,'FontSize',14)
% title(strJ)
% xlabel('Time,h')
% ylabel('Tout, K ')
% 
% subplot(312)
% plot(sol.t,price)
% set(gca,'FontSize',14)
% xlabel('Time,h')
% ylabel('Price, NOK/kWh ')
% 
% subplot(313)
% plot(sol.t,price.*(sol.qhf+sol.qhr))
% set(gca,'FontSize',14)
% xlabel('Time,h')
% ylabel('price*Q, NOK/h')
% 
% figure
% subplot(211)
% plot(sol.t,sol.co2*100,sol.t,100*co2max*ones(size(sol.t)),'red')
% set(gca,'FontSize',14)
% xlabel('Time,h')
% ylabel('CO2 level, % mass')
%  
% subplot(212)
% plot(sol.t,sol.qout)
% set(gca,'FontSize',14)
% xlabel('Time,h')
% ylabel('flow out, Kg/s')
% tilefigs
% pause