%% Opgave 1
clc; clear;
%x0 = [0,4]';
%x0 = [-4,4]';
x0 = [-2,0]';
%x0 = [-3,1]';
y0 = 1;
obj = @ObjFun1;
con = @ConFun1;

[x_sol,stat] = NewtonSQP(obj,con,x0,y0);
x_sol
stat
%con(x_sol)
stat.X

x = -5:0.005:5;
y = -5:0.005:5;
[X,Y] = meshgrid(x,y);
F = (X.^2+Y-11).^2 + (X + Y.^2 - 7).^2;
v = [0:2:10 10:10:100 100:20:200];
[c,h]=contour(X,Y,F,v,'linewidth',2);
colorbar
yc1 = (x+2).^2;
%yc2 = (4*x)/10;
ylim([-5,5])
xlim([-5,5])
hold on
fill(x,yc1,[0.7 0.7 0.7],'facealpha',0.2)
%fill([x x(end) x(1)],[yc2 -5 -5],[0.7 0.7 0.7],'facealpha',0.2)
for i = 2:length(stat.X)
    plot([stat.X(1,i-1),stat.X(1,i)],[stat.X(2,i-1),stat.X(2,i)],'g');
    plot(stat.X(1,1),stat.X(2,1),'r*');
    plot(stat.X(1,i-1),stat.X(2,i-1),'r*');
    plot(stat.X(1,end),stat.X(2,end),'r*');
end
hold off