% function sol= propt
% profile on
clear all
close all
clc

[~,par]=getData;

%Define main parameters
Vmax=150;   %[m^3] equivalent to 200 liters
Vmin=50;   %[m^3] equivalent to 100 liters
Tmax=90+273;
Tmin=50+273;
Qmax=5.5; % [Kw]
qinmax=par.qinmax; %[m^3/s]
cp=par.cp; %[KJ/kg/K]
rho=par.rho; % [kg/m^3]
Tin=par.Tin;% [K]
 V0=par.V0*1000;
% V0=50;
 T0=par.T0;
% T0=60+273;
qout_0=1*4.16;% l/min %[m^3/s] ==> corresponds to 4.16 l/min
%$/kWh%
p0=2;
Tcw=Tin;
Tsp=50+273;
qshower=1.0*qout_0;


%load guess
strpath=pwd;
filename=strcat(strpath,'\OPTdata\penaltyTemp09-Jul-2013.mat');
guess=load(filename);
auxGuess=load('Optguess.mat')
sol_guess=auxGuess.guess;

% sol_guess=guess.sol;
% Tguess=sol_guess.T;
% Vguess=sol_guess.V;
% Qguess=sol_guess.Q;
% qinguess=sol_guess.qin;
% 


t_f=48*3600;

narr=[5 5];

%input derivatives penalty on
gama=1e-8
%quadratic term on
alpha=0;
%away from max capacity penalty on
beta=10/400;
% energy cost on
delta=1
%Terminal cost term on
mu=0*1e15;


% t_f1=5.0*3600;
% t_f2=8*3600;
% t_f3=13*3600;
% t_f4=18*3600;
% t_f5=22*3600;
% 
% dt=1.*20*60;
% dt2=1.*20*60;
% dt3=20*60;
% dt4=20*60;
% dt5=20*60;
% timesvec=[0 t_f1 t_f1+dt t_f2 t_f2+dt2 t_f3 t_f3+dt3 t_f4 t_f4+dt4 t_f5 t_f5+dt5  t_f];
% times=[0 t_f1 t_f1+dt t_f2 t_f2+dt2 t_f3 t_f3+dt3   t_f];
%  timesvec=[0 t_f1/2 t_f1 t_f1+dt  (t_f2+t_f1+dt)/2  t_f2 t_f2+dt2 t_f3  t_f3+dt3 t_f];
%  flows=[0 0 1.5*qout_0 0 0 1.5*qout_0/1 0 qout_0 0];


%on and off flow
%arbitrary profile
% timestart=[t_f1 t_f2 t_f3 t_f4 t_f5];
% duration=0.5*[dt dt2 dt3 dt4 dt5];
% flowsMag=1.2*[qout_0 qout_0 qout_0*0 qout_0 qout_0];

%random profile from data
load usage48h.mat
timestart=usage.times;
duration=usage.durations;
flowsMag=usage.flows;

[ timesvec,flows ] = getTimeIntervals(timestart, duration , flowsMag,  t_f );



numPhases=length(timesvec)-1;

display('============Total number of phases is=============')
numPhases
price_data = tdfread('price_data.dat');
for i=1:length(narr)
    
    
 n=narr(i);
        
    ceqvec={};
    cbndvec={};
    cboxvec={};
    guessvec={};
    linkvec={};
    objective=0;
    qoutvec={};
    % beta=beta/i^2;
    
    for k=1:numPhases
        

        
        dt=timesvec(k+1)-timesvec(k);
        
        if dt<=0
            
            display('Something went terribly wrong')
            return 
        end
            
        
        Timevec{k}=tom(strcat('t',num2str(k)));
        Phases{k} = tomPhase((strcat('p',num2str(k))),Timevec{k}, timesvec(k),dt, n,[],'gauss');
        setPhase(Phases{k});
        
        Tvec{k}=tomState(strcat('T',num2str(k)));
        Vvec{k}=tomState(strcat('V',num2str(k)));
        Qvec{k}=tomControl(strcat('Q',num2str(k)));
        qinvec{k}=tomControl(strcat('qin',num2str(k)));
        
        
        
        %% =============== Setup Phases==============
        setPhase(Phases{k})
        display('entrando loop numPhases')
        k
        T=Tvec{k};
        V=Vvec{k};
        qin=qinvec{k};
        Q=Qvec{k};
        
        if i==1
             nt=n;
            % Initial guess
             x0 = {
                 collocate({
                 T == interp1(sol_guess.t,sol_guess.T,linspace(timesvec(k), timesvec(k+1),n),'nearest','extrap')
                 V==interp1(sol_guess.t,sol_guess.V,linspace(timesvec(k), timesvec(k+1),n),'nearest','extrap')
                 })
                 collocate({
                 Q==interp1(sol_guess.t,sol_guess.Q,linspace(timesvec(k), timesvec(k+1),n),'linear','extrap')
                 qin==interp1(sol_guess.t,sol_guess.qin,linspace(timesvec(k), timesvec(k+1),n),'linear','extrap')
                

                    
%                icollocate({
%                                   T == Tmax
%                                 V==Vmax
%                                 })
%                                 collocate({
%                                 Q==Qmax*0
%                                 qin==qinmax*0
                
                })};
        else
            % Initial guess
            x0 = {icollocate({
                T == Ti{k}
                V==Vi{k}
                })
                collocate({
                Q==Qi{k}
                qin==qini{k}
                })};
            
        end
        
        guessvec={guessvec;x0};
        
%         increase=sum(flows(1:k));
        
         qout=(flows(k))*(Tsp-Tcw)/(T-Tcw);
%          qout=flows(k)*qout_0*increase;
        
        qoutvec{k}=qout;
%         pricevec{k}=(p0+0.5*p0*sign(sin(2*pi*(Timevec{k}-8*3600)/(12*3600)+1.*pi/2)))/3600;
        pricevec{k}=interp1(price_data.t,price_data.p,Timevec{k}/3600+24)/1000;
        %     price1=p0;
        
        
        % Box constraints
        cbox = {Tmin <= collocate(T) <= Tmax
            Vmin <= collocate(V) <= Vmax
            0 <= collocate(Q) <= Qmax
            0<=collocate(qin) <=qinmax};
        
        cboxvec={cboxvec, cbox};
        
        
        % Boundary constraints
        if k==1
            cbnd = {initial({T == T0,V==V0})};
        else
            cbnd={};
        end
        cbndvec={cbndvec, cbnd };
        Tsurr=25+273;
        Qloss=0.2*1e-4*(T-Tsurr);
%     Qloss=0;
      
%         Q=Q-Qloss;
        % ODEs and path constraints
        
%         ceq = collocate({
%             dot(V) == (qin-qout)/(1000*60)
%             dot(T)== 1/(V)*(qin/(60*1000)*(Tin-T)+(Q-Qloss)/(cp*rho))
%             });
        
         ceq = collocate({
            dot(V) == (qin-qout)/(60)
            dot(T)== 1/(V)*(qin/(60)*(Tin-T))+(Q-Qloss)/(V*cp)
            });
        
        ceqvec={ceqvec, ceq};
        
        Vscaled=(V-Vmin)/(Vmax-Vmin);
        Tscaled=(T-Tin)/(Tmax-Tin);
        
        Vscaledmax=1;
        Tscaledmax=1;
        
        Tinscaled=(Tin-Tin)/(Tmax-Tin);
        
%         
%         cap_max=Vscaledmax*cp*(Tscaledmax-Tinscaled);
%         cap=Vscaled*cp*(Tscaled-Tinscaled);
%         
        cap=V*cp*(T-Tin);
        cap_max=Vmax*cp*(Tmax-Tin);
        cap_min=Vmin*cp*(Tmin-Tin);
        
        
        cap= (cap-cap_min)/(cap_max-cap_min);
        cap_maxS=1;
        dC=(cap_maxS-cap)/cap_maxS;
        highestCost=Qmax*3/3600;
%         highestcost=1;
%         highestCost=1;
        lowestCost=0;
        
        %Smooth approximation to  % belowTsp=max(0,Tsp-T);
        dT=Tsp-T;
        belowTsp=(1/2+1/pi*atan(1000*dT))*dT;
%         qcw=qshower-qout;
%         Thw=(qout*T+qcw*Tin)/qshower;
         Qdemand=cp*qshower/(60)*(Tsp-Tin);
         maxCompens= 300/3600;
         
%            discount=exp(-1/240000*Timevec{i});
           discount=1;
        % objective = integrate(price*Q)+beta*integrate(price*Q^2)+100000*integrate(1e9*dot(qin)^2)+1000000*integrate(dot(Q)^2);
%       J = (1-beta)*delta*integrate((pricevec{k}*Q-lowestCost)/highestCost)+beta*integrate(dC^2)+gama*1e1*integrate(1e7*dot(qin)^2)+0*gama*integrate(1e9*dot(Q)^2);
        J = integrate(discount*(pricevec{k}*Q-lowestCost)/highestCost)+integrate(discount*(belowTsp^2*Qdemand*beta/3600)/(maxCompens) )+gama*1e1*integrate(1e7*dot(qin)^2)+gama*integrate(1e9*dot(Q)^2)+0*integrate((V-Vmax)^2);
        objective=objective+J;
        
        if k>1
            link={final(Phases{k-1},Vvec{k-1})==initial(Phases{k},Vvec{k})
                final(Phases{k-1},Tvec{k-1})==initial(Phases{k},Tvec{k})};
            
            linkvec={linkvec , link};
        end
        
    end
    %% ===============Solve Optimization Problem========================
    objective=objective+mu*final((cap_maxS-cap)^2);
   finalconst={final(cap==cap_maxS)};
% finalconst={};
    
    options = struct;
    options.name = 'Water heater problem';
    constr = {cboxvec, cbndvec, ceqvec,linkvec,finalconst};
    %     options.scale='auto';
%      options.solver='npsol';
    options.prilev=2;
    
    options.MaxIter=10000;
    options.proptIterations=1;
    options.TolCon=1e-7;
    
    options.ADObj = 1;
    options.ADCons = 1;
%       if i==1
%            [solution]=feasibilitycheck(constr,guessvec);
%       else
        
        [solution,res] = ezsolve(objective, constr, guessvec, options);
%       end
    
    
    
    
    %% ===============Retrieve result========================
    
    
    for v=1:numPhases
        
        ti{v} = subs(collocate(Phases{v},Timevec{v}),solution);
        Ti{v} = subs(collocate(Phases{v},Tvec{v}),solution);
        Vi{v} = subs(collocate(Phases{v},Vvec{v}),solution);
        Qi{v} = subs(collocate(Phases{v},Qvec{v}),solution);
        qini{v} = subs(collocate(Phases{v},qinvec{v}),solution);
        pricei{v} = subs(collocate(Phases{v},pricevec{v}),solution);
        qouti{v}=subs(collocate(Phases{v},qoutvec{v}),solution);
        
        
        
        
    end
    
end

  diary OFF
%%
% t=[ti1;ti2];
% T=[Ti1;Ti2];
% V=[Vi1;Vi2];
% Q=[Qi1;Qi2];
% qin=[qini1;qini2];
% qout=[qout1;qout2];
t=mycell2array(ti);
T=mycell2array(Ti);
Q=mycell2array(Qi);
V=mycell2array(Vi);
qin=mycell2array(qini);
price=mycell2array(pricei);
qout=mycell2array(qouti);
cap=(cp*V.*(T-Tin)-cap_min)/(cap_max-cap_min);
% t=reshape(cell2mat(ti),numPhases*length(cell2mat(ti)),1);
% T=reshape(cell2mat(Ti),numPhases*length(cell2mat(Ti)),1);
% Q=reshape(cell2mat(Qi),numPhases*length(cell2mat(Qi)),1);
% V=reshape(cell2mat(Vi),numPhases*length(cell2mat(Vi)),1);
% qin=reshape(cell2mat(qini),numPhases*length(cell2mat(qini)),1);
% price=reshape(cell2mat(pricei),numPhases*length(cell2mat(pricei)),1);
% qout=reshape(cell2mat(qouti),numPhases*length(cell2mat(qouti)),1);
% cap=(cp*V.*(T-Tin)-cap_min)/(cap_max-cap_min);



sol.t=t;
sol.T=T;
sol.V=V;
sol.qin=qin;
sol.Q=Q;
sol.qout=qout;
sol.price=price;
sol.beta=beta;
sol.gama=gama;
sol.alpha=alpha;
sol.mu=mu;

  if (res.ExitFlag==0)
     str=date;
     strpath=pwd;
     filename=strcat(strpath,'\OPTdata\penaltyTemp48h',str);
     save(filename,'sol')
  end
t=t/3600;
xlimite=[0 t(end)];
figure(1)

Tmaxv=Tmax+zeros(size(T));
Tminv=Tmin+zeros(size(T));
tdist=[timesvec(2) timesvec(2)]./3600;
Tvert=[Tmin Tmax];

tminT=[8.54 8.54];
subplot(3,1,2)
plot(t,T,'b',t,Tmaxv,'r',t,Tminv,'r',tdist,Tvert,'k-.',tminT,Tvert,'r-.');
xlabel('Time, h')
ylabel('Temperature')
xlim(xlimite)
axis tight
Vminv=Vmin+zeros(size(t));
Vmaxv=Vmax+zeros(size(t));
Vvert=[Vmin Vmax];
tminV=[9.34 9.34];
subplot(3,1,1)
plot(t,V,t,Vminv,'r',t,Vmaxv,'r',tdist,Vvert,'k-.',tminV,Vvert,'g-.');
xlabel('Time, h')
ylabel('Volume')
xlim(xlimite)
axis tight
capmax=0*cp*Vmax*(Tmaxv-Tin);
capmin=0*cp*Vmin*(Tminv-Tin);
cvert=[max(capmax) min(capmin)];
tminC=[9.34 9.34];
subplot(3,1,3)
plot(t,cap,t,capmax,'r',t,capmin,'r',tdist,cvert,'k-.',tminT,cvert,'r-.',tminV,cvert,'g-.');
xlabel('Time, h')
ylabel('Capacity')
xlim(xlimite)
axis tight
figure(2)
Qvec=[0 Qmax];
qinvec=[0 qinmax];
subplot(2,1,1)
plot(t,Q,tdist,Qvec,'k-.',tminT,Qvec,'r-.',tminV,Qvec,'g-.');
xlabel('Time, h')
ylabel('Power, kW')
xlim(xlimite)

subplot(2,1,2)
qin_ub=zeros(size(qin))+qinmax;
plot(t,qin,t,qin_ub,'r',tdist,qinvec,'k-.',tminV,qinvec,'g-.',tminT,qinvec,'r-.');
xlabel('Time, h')
ylabel('Flow in, l/min')
xlim(xlimite)

figure(3)
subplot(211)
plot(t,qout)
xlabel('Time, h')
ylabel('Water usage, l/min')
xlim(xlimite)

subplot(212)
plot(t,price)
xlabel('Time')
ylabel('Price')
xlim(xlimite)



% % Simulate model
% figure(4)
% simulate(sol)
 tilefigs

% pause
% runSimulink;