warning off
clear all, clc, close all


%Model structure
housereal=getModel;
housepred=getModel;
%will use prediction data for outdoor temperature
    housepred.Parameters.Value=1;
 housereal.Parameters.Value=1;

%get parameters and initial states
[x0,par]=getData;
initialStates=x0;

%%
%Optimize trajectory
tinit=0;
totalIterations=1;
notConverged=0;
simtime=24*3600;
horizon=8*24*3600;
tdist=2*3600;

%  load sol_Iguess55
sol_aux=open('solMH.mat');
sol_guess=sol_aux.solMH;
u0=[sol_guess.qhf(1) sol_guess.qhr(1) sol_guess.qair(1)];
[sol,res] =propt_room_1Q(par,initialStates,u0,tinit,sol_guess,simtime,horizon);


%%
Iter=res.Iter;
% Sampling time 10 min
Ts=2*60*3600/60;
uopt=[sol.qhf(:) sol.qhr(:) sol.qair(:)];
time=linspace(sol.t(1),tinit+Ts,100);
usim = interp1(sol.t(:),uopt,time,'linear','extrap');
U  = iddata([], usim , 'Ts', [],'SamplingInstants',time);

% Simulate
[Y, Yd,xfinal]=sim(housereal,U,initialStates);
initialStates=xfinal(end,:)';

%%

Yvec=[Y.OutputData];
Uvec=[usim];
Timevec=[time];
tinit=time(end)+1;

% Real time plotting
plotRealTime
pause(1)
drawnow


%  figure(1) 
% numframes=simtime/Ts; 
%  outputs=moviein(numframes); % create the movie matrix 
%  set(gca,'NextPlot','replacechildren') 
%  axis equal % fix the axes 
%  outputs(:,totalIterations)=getframe; 



while tinit<=simtime
    
    %Problem with the initial condition! check initial guess
    %     initialStates(2)=max(290,initialStates(2));
    % initialStates(1)=max(273,initialStates(1));
    %     initialStates(4)=min(par.co2max,initialStates(4));
    u0=usim(end,:);
    states=[sol.Tf sol.Tr sol.mr sol.co2];
    iniStates= interp1(sol.t,states,time(end),'nearest');
       
    %Use
    guess=getGuess(housepred,initialStates,sol,tinit,horizon);
    
    [sol,res] =propt_room_1Q(par,initialStates,u0,tinit,guess,simtime,horizon);
    
    if (res.ExitFlag~=0)&&(res.ExitFlag~=3)
        %try again with different initial guess
        guess=getGuess(housepred,initialStates,sol,tinit,horizon);
        [sol,res] =propt_room_1Q(par,initialStates,u0,tinit,guess,simtime,horizon);
        
        if (res.ExitFlag~=0)&&(res.ExitFlag~=3)
            
            display('Something went wrong')
            res.ExitFlag
            res.ExitText
            %Use previous solution at correct time
            sol=getGuess(housepred,initialStates,guess,tinit,horizon);
            notConverged=notConverged+1
        end
    end
    
    
    % Prepare data
    uopt=[sol.qhf(:) sol.qhr(:) sol.qair(:)];
    time=linspace(sol.t(1),min(tinit+Ts,simtime),100);
    usim = interp1(sol.t(:),uopt,time,'linear');
    U  = iddata([], usim , 'Ts', [],'SamplingInstants',time);
    
%     %Disturbance
%     if (tinit>=tdist)&&(tinit<=tdist+5*Ts)
%         housereal.Parameters.Value=1.5;
        % Simulate model with disturbance
        [Y,Yd,xfinal]=sim(housereal,U,initialStates);
        
%     else
%         housereal.Parameters.Value=1.0;
%         % Simulate model without disturbance
%         [Y,Yd,xfinal]=sim(housereal,U,initialStates);
%         
%      end
    
    
    initialStates=xfinal(end,:)';
    tinit=time(end)+1;
    Timevec=[Timevec time];
    Yvec=[Yvec; Y.OutputData];
    Uvec=[Uvec;usim];
    totalIterations=totalIterations+1
    Iter=res.Iter+Iter;
    
    % Real time plotting
    plotRealTime
    pause(1)
    drawnow
    
%      figure(1) 
%  set(gca,'NextPlot','replacechildren') 
%  axis equal % fix the axes 
%  outputs(:,totalIterations)=getframe; 

    
end

%%
% figure(10) 
% movie(outputs,10,3) % Play the MATLAB movie 
% 
%  
%  save movie.mat outputs % save the MATLAB movie to a file 

 
 
totalIterations
notConverged
averageIterations=Iter/totalIterations

% figure(1)
%   set(gcf, 'PaperPosition', [0 0 7 5]); %Position plot at left hand corner with width 5 and height 5.
% set(gcf, 'PaperSize', [7 5]); %Set the paper to have width 5 and height 5.
% saveas(gcf, 'C:\Users\viniciud\Documents\Vinicius\PhD NTNU\courses\Numerical Optimal Control\Project\Report\pics\dis_outputs', 'epsc') %Save figure
% 
% figure(2)
%   set(gcf, 'PaperPosition', [0 0 7 5]); %Position plot at left hand corner with width 5 and height 5.
% set(gcf, 'PaperSize', [7 5]); %Set the paper to have width 5 and height 5.
% saveas(gcf, 'C:\Users\viniciud\Documents\Vinicius\PhD NTNU\courses\Numerical Optimal Control\Project\Report\pics\dis_inputs', 'epsc') %Save figure
% 
% figure(3)
%   set(gcf, 'PaperPosition', [0 0 7 5]); %Position plot at left hand corner with width 5 and height 5.
% set(gcf, 'PaperSize', [7 5]); %Set the paper to have width 5 and height 5.
% saveas(gcf, 'C:\Users\viniciud\Documents\Vinicius\PhD NTNU\courses\Numerical Optimal Control\Project\Report\pics\dis_disturbance', 'epsc') %Save figure

%% Plotting functions
Tf=Yvec(:,1);
Tr=Yvec(:,2);
mr=Yvec(:,3);
co2=Yvec(:,4);
EnergyCost=Yvec(:,5);
To=Yvec(:,6);
price=Yvec(:,7);
qhf=Uvec(:,1);
qhr=Uvec(:,2);
qair=Uvec(:,3);



solMH.t=Timevec;
solMH.qhf=qhf;
solMH.qhr=qhr;
solMH.mr=mr;
solMH.qair=qair;
solMH.Tf=Tf;
solMH.Tr=Tr;
solMH.co2=co2;
solMH.J=EnergyCost;
solMH.price=price;
solMH.To=To;
solOPT=solMH;
Cost=EnergyCost(end);
% save('solOPT','solOPT')


 
