%% TMA671 Lösning av uppgift N.7.10
clear;
% Under storgruppsövningen som hölls onsdagen den 21e Maj 2018
% så demonstrerades uppgift 10 i Kapitel 7 i Numerisk Analys
% boken. Uppgiften handlar om att minimera en konvex funktion 
% f:R^2->R med hjälp av Newtons metod.
% Programmet är skrivet av Johannes Borgqvist

%% Almänna saker för uppgiften
% Funktion som skall minimeras
f = @(x,y) (0.5.* (x.^2 - y).^2) + (0.5 .* (1 - x) .^2);
% Definiera minimipunkt 
xMin = [1; 1];
% Definiera gradient
gradF =@(x,y) [(2.*x.^3) + ((1-2.*y).* x) - 1;...
                y - x.^2];
% Definiera Hessianen
hessCol1 = @(x,y) [((6.*x.^2)-(2.*y) + 1);...
            (-2.*x)];% Kolonn 1
hessCol2 = @(x,y) [(-2.*x);...
            (1)]; % Kolonn 2        
% Definiera en startgissning
x0 = [2; 2];

% Första iteration
iteration = 0;

% Skriv ut iteration 0
fprintf('====================================================================\n\n');
fprintf('Iteration\t\tx\t\tf(x,y)\t\tAvstånd\n\n');
fprintf('====================================================================\n\n');
fprintf('%d\t\t(%0.2f,%0.2f)\t\t%0.4f\t\t%0.4f\n\n',...
    iteration,x0(1),x0(2),f(x0(1),x0(2)),sqrt((x0-xMin)'*(x0-xMin)));


%% Iteration 1: Vi tar ett steg med Newton Raphson
% Uppdatera iterationen
iteration = iteration + 1;
% Beräkna stegriktning
s = -gradF(x0(1),x0(2));
% Beräkna Hessianen
H = [hessCol1(x0(1),x0(2)), hessCol2(x0(1),x0(2))];
% Beräkna stegriktningen
p = H\s;
% Beräkna steglängden
alpha = ((s' * p) /(p'*(H*p)));
% Uppdatera vår startgissning
x1 = x0 + alpha*p;
% Skriv ut
fprintf('%d\t\t(%0.2f,%0.2f)\t\t%0.4f\t\t%0.4f\n\n',...
    iteration,x1(1),x1(2),f(x1(1),x1(2)),sqrt((x1-xMin)'*(x1-xMin)));

%% Iteration 2: Vi tar ett steg med Newton Raphson
% Uppdatera iterationen
iteration = iteration + 1;
% Beräkna stegriktning
s = -gradF(x1(1),x1(2));
% Beräkna Hessianen
H = [hessCol1(x1(1),x1(2)), hessCol2(x1(1),x1(2))];
% Beräkna stegriktningen
p = H\s;
% Beräkna steglängden
alpha = ((s' * p) /(p'*(H*p)));
% Uppdatera vår startgissning
x2 = x1 + alpha*p;
% Skriv ut
fprintf('%d\t\t(%0.2f,%0.2f)\t\t%0.4f\t\t%0.4f\n\n',...
    iteration,x2(1),x2(2),f(x2(1),x2(2)),sqrt((x2-xMin)'*(x2-xMin)));

%% Plottar
% number of elements
nuOfEle = 100;
% Define the boundaries
minX = 0.8; maxX = 2.5; minY = 0.3; maxY = 3.4;
% Define the function mesh
[functionMesh,xMesh,yMesh] = meshGenerator(f,minX,maxX,minY,maxY,nuOfEle); 


fig = figure('units','normalized','outerposition',[0 0 1 1],...
     'PaperPositionMode','auto');
%figure(1)
clf
contourf(xMesh,yMesh,functionMesh)
colormap jet
cb = colorbar;
%cb.Label.Interpreter = 'latex';
set(cb,'Ticklabelinterpreter','latex','Fontsize',20)
hold on
p1 = plot(xMin(1),xMin(2), '*','Color',[1, 99/255 ,71/255], 'MarkerSize', 30, 'LineWidth', 3);
hold on
p2 = plot([x0(1); x1(1); x2(1)],[x0(2); x1(2); x2(2)], '*red', 'MarkerSize', 30, 'LineWidth', 3);
hold on
p3 = plot(x1(1),x1(2), '*green', 'MarkerSize', 30, 'LineWidth', 3);
hold on
p4 = plot(x2(1),x2(2), '*magenta', 'MarkerSize', 30, 'LineWidth', 3);
hold off
axis([minX, maxX, minY, maxY])
set(gca,'Ticklabelinterpreter','latex','Fontsize',20)
leg = legend([p1,p2,p3,p4],'Minimum','Startgissning','Iteration 1','Iteration 2');
set(leg,'interpreter','latex','Fontsize',30,'Location','NorthWest')
xlabel('$x$','interpreter','latex','FontSize',40)
ylabel('$y$','interpreter','latex','FontSize',40)
%clabel('$f\left(x,y\right)$','interpreter','latex','FontSize',40)
cb.Label.String = '$f\left(x,y\right)$';
cb.Label.Interpreter = 'latex';
cb.Label.FontSize = 40;
titleStr = {'\textbf{TMA671 Uppgift N.7.10}','Datum: 2018-05-21, Skriven av: Johannes Borgqvist',...
    ['\textit{Minimering med Newtons metod:}'],...
    '$f(x,y)=\frac{1}{2}\left(x^2-y\right)^2+\frac{1}{2}\left(1-x\right)^{2}$'
    };
title(titleStr,'interpreter','latex','Fontsize',50)
print(fig, '-depsc', '-r0', 'UppgiftN710Datum20180521.eps')


fprintf('====================================================================\n\n');