%
% Solve Optimal Control Problem
% 
% Min 0.5 * int_[0.4,0.6] ( y(x) - ydes(x) )^2 dx + 0.5*alha * int_[0.1,0.3] u(x)^2 dx
%
% subject to
%
%     - diff  y''(x) + adv y'(x) + react y(x) = f(x) + u(x)|[0.1,0.3]   x in (0,1),   
%
%                                        y(0) = g0,
%                                        y(1) = g1,
%
% Matthias Heinkenschloss
% Jan 14, 2020
%


set(0, 'defaultaxesfontsize',28,'defaultaxeslinewidth',3.0,...
       'defaultlinelinewidth',3.0,'defaultpatchlinewidth',3.0,...
       'defaulttextfontsize',28,'defaulttextinterpreter','latex');


%% problem data
diff  = 1.e-1;
adv   = 1;
react = 0;
g0    = 0;
g1    = 0;
alpha = 1.e-3; % control penalty

f     = inline( 'ones(size(x))', 'x', 'diff', 'adv', 'react');
f     = inline( 'zeros(size(x))', 'x', 'diff', 'adv', 'react');
y_des = inline( 'ones(size(x))', 'x');

% set mesh points
k   = 20;        % number of grid points plus one is multiple of 10
n   = (10*k-1);   
m   = 2*k+1;
h   = 1/(n+1);
x   = (0:h:1)';

%% upwind finite difference approximation
if( adv < 0 )
    error( 'adv < 0 not yet implemented')
end
% system matrix
c    = -(diff/h^2 + adv/h)*ones(n,1);
d    = (2*diff/h^2 + adv/h + react)*ones(n,1);
e    = (-diff/h^2)*ones(n,1);
A    = spdiags([c d e], -1:1, n, n);
% right hand side
b    = f(x(2:n+1), diff, adv, react);
b(1) = b(1) + (diff/h^2 + 0.5*adv/h)*g0;
b(n) = b(n) + (diff/h^2)*g1;

B = sparse(n,m);
B(1*k:3*k,:) = -speye(m);
    
e = h*ones(2*k+1,1); %e(1)=e(1)/2; e(end)=e(end)/2;
R = diag(e);
z = zeros(n,1); 

z(4*k:6*k) = e;
Q = diag(z);



%% solve optimal control problem
K   = [ Q            zeros(n,m)    A'
        zeros(m,n)   alpha*R       B'
        A            B             zeros(n,n)];
  
rhs = [Q*y_des(x(2:n+1));
       zeros(m,1);
       b];

sol = K\rhs;

y = sol(1:n);
u = sol(n+1:m+n);
p = sol(n+m:2*n);
    

  
% plot solution
figure(1)
plot(x,[g0;y;g1],'b-'); hold on
plot(x(4*k+1:6*k+1),y_des(x(4*k+1:6*k+1)),'r--'); hold off
xlabel('$x$')
ylabel('$y(x)$')
axis([-0.1,1.1,-0.1,1.1])
filename_sol = ['ocp_bvp_1d_state_diff',num2str(log10(diff)),'n',int2str(n),'.eps'];
%print('-depsc',filename_sol)
figure(2)
plot(x(k+1:3*k+1),u,'b-');
xlabel('$x$')
ylabel('$u(x)$')
axis([-0.1,1.1,floor(min(u)),ceil(max(u))])
filename_sol = ['ocp_bvp_1d_control_diff',num2str(log10(diff)),'n',int2str(n),'.eps'];
%print('-depsc',filename_sol)



