% Aufgabe 1
% Strafkosten-Verfahren

clear all;
fprintf('Programmieraufgabe 1: Strafkosten-Verfahren\r')
pin=input('Bitte erste Koordinate des Startvektors angeben: ');
x=[pin;7];  %Startvektor

epsilon = 10^-4;


f = @(x) 16*x(1)^4 - 32*x(1)^2*x(2) - 607*x(1)^2 + 32*x(2)^2 + 8*x(1)*x(2) + 1152*x(2) + 136*x(1);
df = @(x) [	4*16*x(1)^3 - 2*32*x(1)*x(2) - 2*607*x(1) + 8*x(2) + 136;
			-32*x(1)^2 + 2*32*x(2) + 8*x(1) + 1152];
			
g = @(x) [-20 - x(1) - x(2); x(1) + x(2) + 1];
dg = @(x) [-1 -1; 1 1];
gp = @(x) [max(g(x), 0)];

h = @(x) 4*x(1)*x(2) + 68*x(1) + x(2) + 19;
dh = @(x) [4*x(2) + 68 ; 4*x(1) + 1];


p = @(x) sum(gp(x).^2) + sum(h(x).^2);
dp = @(x) 2*dg(x)'*gp(x) + 2*h(x)'*dh(x);

r = @(k) 10^k;


phi = @(x,k) f(x) + r(k)*p(x);
dphi = @(x,k) df(x) + r(k)*dp(x);

[x, k] = strafkosten(f, df, p, dp, r, x, epsilon);

fprintf('\n########################################################\n');
fprintf('pin		= %s\n', mat2str(pin));
fprintf('k		= %s\n', mat2str(k));
fprintf('x*		= %s\n', mat2str(x));
fprintf('f(x*)		= %s\n', mat2str(f(x)));
fprintf('phi(x*)		= %s\n', mat2str(phi(x, k)));
fprintf('dphi(x*)	= %s\n', mat2str(dphi(x, k)));
