%%% GODDARD 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 30-33, Chapter 4

function out=main_goddard
clear all;clc;close all;
global eps0 nu qmax
EPS=logspace(-2,-9,71);



figure(1);
set(gcf,'position',[83 47 1080 662]); clf; drawnow
subplot(3,2,1:2);grid on;hold all;title('Histories of optimal altitude h^*_\epsilon(t)');xlabel('time t');ylabel('altitude h')
subplot(3,2,3);grid on;hold all;title('Histories of penalized optimal control u^*_\epsilon(t)');xlabel('time t');ylabel('thrust u')
subplot(3,2,4);grid on;hold all;title('Histories of dynamic pressure q^*_\epsilon(t)');xlabel('time t');ylabel('dynamic pressure q(h,v)')
subplot(3,2,5);grid on;hold all;title('Histories of velocity v^*_\epsilon(t)');xlabel('time t');ylabel('velocity v')
subplot(3,2,6);grid on;hold all;title('Histories of rocket mass m^*_\epsilon(t)');xlabel('time t');ylabel('mass m')
informations=uicontrol('style','text','position',[0 10 400 20],'string','Ready'); drawnow


%Initialization
qmax=15;
T=1;
solinit=bvpinit([0:.01:1],@yeval);
option=bvpset('Nmax',20000,'AbsTol',1e-6,'RelTol',1e-7);
eps0=EPS(1);
set(informations,'string',['Solving POCP with relaxed heat constraint']); drawnow     
disp('Solving POCP with relaxed heat constraint')
SOL=bvp5c(@eq_diff,@bcfct,solinit,option);

%Start of the interior point algorithm
qmax=10;
option=bvpset('Nmax',200000,'AbsTol',1e-8,'RelTol',1e-7);

for i=1:length(EPS)
    eps0=EPS(i);
    set(informations,'string',['Solving POCP for epsilon =' num2str(eps0)]); drawnow     
    disp(['Solving POCP for epsilon =' num2str(eps0)])
    SOL=bvp5c(@eq_diff,@bcfct,SOL,option);
    if eps0==1e-2 || eps0==1e-3 || eps0==1e-4 || eps0==1e-6 || eps0==1e-9
        
        nu=asinh(-((SOL.y(6,:).*SOL.y(4,:))./SOL.y(3,:) - 2*SOL.y(7,:).*SOL.y(4,:))/eps0);
        u=(1+tanh(nu*2/3.5))*3.5/2;
        q=(6200*SOL.y(2,:).^2.*exp(500*(1-SOL.y(1,:))));
        wd=1; if i==length(EPS); wd=2; end
        subplot(3,2,1:2);plot(SOL.x*SOL.y(4,end),SOL.y(1,:),'linewidth',wd)
        subplot(3,2,3);plot(SOL.x*SOL.y(4,end),u,'linewidth',wd)
        subplot(3,2,4);plot(SOL.x*SOL.y(4,end),q,'linewidth',wd)
        subplot(3,2,5);plot(SOL.x*SOL.y(4,end),SOL.y(2,:),'linewidth',wd)
        subplot(3,2,6);plot(SOL.x*SOL.y(4,end),SOL.y(3,:),'linewidth',wd)
        drawnow
    end
end
subplot(3,2,1:2);legend('\epsilon=10^{-2}','\epsilon=10^{-3}','\epsilon=10^{-4}','\epsilon=10^{-6}','\epsilon=10^{-9}')
subplot(3,2,3);legend('\epsilon=10^{-2}','\epsilon=10^{-3}','\epsilon=10^{-4}','\epsilon=10^{-6}','\epsilon=10^{-9}')
subplot(3,2,4);legend('\epsilon=10^{-2}','\epsilon=10^{-3}','\epsilon=10^{-4}','\epsilon=10^{-6}','\epsilon=10^{-9}')
subplot(3,2,5);legend('\epsilon=10^{-2}','\epsilon=10^{-3}','\epsilon=10^{-4}','\epsilon=10^{-6}','\epsilon=10^{-9}')
subplot(3,2,6);legend('\epsilon=10^{-2}','\epsilon=10^{-3}','\epsilon=10^{-4}','\epsilon=10^{-6}','\epsilon=10^{-9}')

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

function [dxdt]=eq_diff(x,y)
    global eps0 qmax
    T=y(4);
    nu=asinh(-T/eps0*(y(6)/y(3) - 2*y(7)));
    u=3.5/2*(tanh(2*nu/3.5)+1);
    temp_e=exp(500*(1-y(1)));
    D=310*y(2)^2*temp_e;
    q=20*D;

    f1 =T*y(2);
    f2 =T*( (u - D)/y(3)-1/y(1)^2 );
    f3 =-T*2*u;
    f4 = 0;
    f5 =y(6)*T*( -500*D/y(3) -2/y(1)^3)+eps0*(10000*D)*dgamma_g(q-qmax,1.1); %
    f6 =T - T*y(5) + (620*y(6)*y(2)*T*exp(500-500*y(1)))/y(3)-eps0*12400*y(2)*exp(500-500*y(1))*dgamma_g(q-qmax,1.1);
    f7 =T*y(6)*(u - D)/y(3)^2;
    f8 =y(2) + 2*y(7)*u - y(5)*y(2) + y(6)*(1/y(1)^2 - (u - D)/y(3));

    dxdt=[f1;f2;f3;f4;f5;f6;f7;f8];
end

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

function res = bcfct(ya,yb)
    res = [ya(1)-1;ya(2);ya(3)-1;yb(3)-.6;yb(5);yb(6);ya(8);yb(8)];
end

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



