
h = 0.1;


sig_fct = @(gam,x) -gam*x;
mu_fct = @(gam,x) gam;
f_fct = @(gam,x,t) -pi*cos(pi*t)*25*(x^2) ;%1-x^2;
Gamma = [0,0.5,1];



x_vec = -1:h:1;
t_vec = 0:h:1;

n_x = length(x_vec);
n_t = length(t_vec);
n_it = 30;

G = zeros(n_t,n_x);

u_sol = @(t,x) max(sin(pi*t)*(5*x)^2,20*t) ;
u_num = zeros(n_t,n_x); % I assumed boundary conditions to be zero
u_num(n_t,:) = 0;
u_num(:,1) = sin(pi*t_vec(:))*25 ;
u_num(:,n_x) = sin(pi*t_vec(:))*25 ;




for it = 1:n_it
    
    for t = n_t -1 :-1:1
        for i = 2:n_x-1
    
         gamma = G(t,i);
          
         % find u with Newton
         x = u_num(t,i);
         for newton = 1:30
             x = x -  FHJB(h,u_num(t+1,i),x,u_num(t,i+1),u_num(t,i-1),mu_fct(gamma,x_vec(i)),f_fct(gamma,x_vec(i),t_vec(t)),sig_fct(gamma,x_vec(i)))/...
                 Fder(h,u_num(t+1,i),x,u_num(t,i+1),u_num(t,i-1),mu_fct(gamma,x_vec(i)),f_fct(gamma,x_vec(i),t_vec(t)),sig_fct(gamma,x_vec(i)));
             
         end
         u_num(t,i) = x;
         
         % get the gamma
         pp = Hsup(h,u_num(t+1,i),u_num(t,i),u_num(t,i+1),u_num(t,i-1),mu_fct(gamma,x_vec(i)),f_fct(gamma,x_vec(i),t_vec(t)),sig_fct(gamma,x_vec(i)));
         for q = 1:length(Gamma)
         
             gamma = Gamma(q);
             
             if pp < Hsup(h,u_num(t+1,i),u_num(t,i),u_num(t,i+1),u_num(t,i-1),mu_fct(gamma,x_vec(i)),f_fct(gamma,x_vec(i),t_vec(t)),sig_fct(gamma,x_vec(i)))
             pp = Hsup(h,u_num(t+1,i),u_num(t,i),u_num(t,i+1),u_num(t,i-1),mu_fct(gamma,x_vec(i)),f_fct(gamma,x_vec(i),t_vec(t)),sig_fct(gamma,x_vec(i)));
             G(t,i) = gamma;
             end
             
         end
         
         
        end
    end
end



u_num(1,:) = 0;

% plot
subplot(2,1,1)
surf(t_vec,x_vec,u_num');
zlim([0,25]);
xlabel('t');
ylabel('x');

subplot(2,1,2);
ezsurf(u_sol,[0, 1, -1, 1]);
