%%% TOY PROBLEM
%%%
%%%
%%% 07/27/2012
%%% cite:  "Dynamic control of energy in buildings using constrained
%%% optimal control by interior penalty"
%%% Paul Malisani, PhD Dissertation, MINES ParisTech, 2012
%%% see pages 23-24, Chapter 4


function out=main
close all;clear all;clc

global eps ng X Y uopt topt

t0=[0:.1:5];
ng=1.1;
EPS=logspace(-1,-7,67); % This setting yields : {1e-1 , 1e-2 , 1e-3 , 1e-4 , 1e-5 , 1e-6 , 1e-7} belong to the set EPS
options_ode=odeset('MaxStep',2e-3);
figure(1);
set(gcf,'position',[83 47 1080 662]); clf; drawnow
subplot(221);grid on;hold all;title('Histories of optimal position x^*_\epsilon while decreasing \epsilon');xlabel('time');ylabel('x^*(t)')
subplot(222);grid on;hold all;title('Histories of optimal control u^*_\epsilon while decreasing \epsilon');xlabel('time');ylabel('u^*(t)')
subplot(223);grid on;hold all;title('Histories of optimal first adjoint state p^*_{1,\epsilon} while decreasing \epsilon');xlabel('time');ylabel('p_1(t)')
subplot(224);grid on;hold all;title('Histories of optimal first adjoint state p^*_{2,\epsilon} while decreasing \epsilon');xlabel('time');ylabel('p_2(t)')
informations=uicontrol('style','text','position',[0 10 400 20],'string','Ready'); drawnow     
 
solinit=bvpinit(t0,@yeval);

for j=1:length(EPS)    
    % solving POCP
    eps=EPS(j);
    set(informations,'string',['Solving POCP for epsilon =' num2str(eps)]); drawnow     
    disp(['Solving POCP for epsilon =' num2str(eps)])
    options=bvpset('AbsTol',eps/10,'RelTol',eps/10,'NMax',200000,'FJacobian',@jac_bvp);
    SOL=bvp5c(@ode_bvp,@twobc,solinit,options);
    X=SOL.x;
    Y=SOL.y;
    solinit=bvpinit(SOL.x,@yeval2); 
    h=mod(-log(eps)/log(10),2)-1;
    
    % display of intermediate solution
    if abs(h)<=1e-3
        % To display the state x_1(t) we use the optimal control u computed
        % using bvp5c and simulate it using the command ode15s
        uopt=tanh(asinh(-SOL.y(4,:)/eps));
        topt=SOL.x;
        set(informations,'string','Recomputing ode solution for display');       
        disp('Recomputing ode solution for display')
       	[tout,yout]=ode15s(@ode,0:.01:5,[1;0],options_ode);
        % Display of the obtained variables
        wd=1; if j==length(EPS); wd=2; end 
        subplot(221);plot(tout,yout(:,1),'linewidth',wd)
        subplot(222); plot(SOL.x,uopt,'linewidth',wd)
        subplot(223); plot(SOL.x,SOL.y(3,:),'linewidth',wd)
        subplot(224); plot(SOL.x,SOL.y(4,:),'linewidth',wd)
        drawnow
    end
    
end

subplot(221);legend('\epsilon=10^{-1}','\epsilon=10^{-3}','\epsilon=10^{-5}','\epsilon=10^{-7}');
subplot(222);legend('\epsilon=10^{-1}','\epsilon=10^{-3}','\epsilon=10^{-5}','\epsilon=10^{-7}');
subplot(223);legend('\epsilon=10^{-1}','\epsilon=10^{-3}','\epsilon=10^{-5}','\epsilon=10^{-7}');
subplot(224);legend('\epsilon=10^{-1}','\epsilon=10^{-3}','\epsilon=10^{-5}','\epsilon=10^{-7}');

set(informations,'string','Done')

end


function dg=dgamma(x,n)
if x<=0
    dg=0;
else
    dg=-n*(x)^(-n-1);
end
end

function d2g=d2gamma(x,n)
if x<=0
    d2g=0;
else
    d2g=n*(n+1)*(x)^(-n-2);
end
end


function [dydx]=ode_bvp(x,y)
global eps ng
xiopt=-asinh(y(4)/eps);
uopt=tanh(xiopt);
dydx=[y(2); uopt; -2*y(1)-eps*dgamma(-.2+y(1),ng); -y(3);];
end


function [dydx]=ode(x,y)
global uopt topt
u=interp1(topt,uopt,x,'nearest');
dydx=[y(2); u];
end


function [dfdx]=jac_bvp(x,y)
global eps ng
dfdx=[0 1 0 0;...
      0 0 0 -1/eps*1/sqrt((y(4)/eps)^2+1)*1/cosh(asinh(-y(4)/eps))^2;...
      -2-eps*d2gamma(y(1)-.2,ng) 0 0 0;...
      0 0 -1 0];
end

function res =twobc(ya,yb)
res=[ya(1:2)-[1;0];yb(3:4)];
end

function Y=yeval(xi)
Y=[1;1;0;0];
end

function Yv=yeval2(xi)
global X Y
Yv=[interp1(X,Y',xi)]';
end
