% Loese das Problem min{ f(x) : Ax <= b, Cx = c }
% mittels Methode der projizierten Gradienten.
%
% Parameter:
% f		Zielfunktion mit f : R^n -> R
% df	Gradient von f (als Spalte geschrieben!)
% A		m x n Matrix
% b		Vektor aus R^m
% C		s x n Matrix
% c		Vektor aus R^s
% x		x aus M, Startvektor
%
% epsilon	Genauerigkeitskonstante
%
% Rueckgabewerte:
% x		Minimalstelle
% k		Anzahl der Iterationen
%
% Das Verfahren berechnet nur die ersten 25 Iterationsschritte!!
%
% Annahmen: 
% Die Zielfunktion ist quadratisch.
% 	Ansonsten Bisektion, Hybrid-Newtow usw. fuer das 
% 	eindimensinale Unterproblem verwenden.
% Die Matrix J hat immer vollen Rang.
%	Ansonsten Updatingformel verwenden.
%
function [x,k] = projGrad(f, df, A, b, C, c, x, epsilon);

K = 25;
[m, n] = size(A);
[s, n] = size(C);

for k=1:K
	
	%%
	% A Best. der Abstiegsrichtung
	%%
	
	%%%%
	% a
	
	% J sei die Matrix der aktiven Restriktionsgradienten
	% fuer x.
	
	activeIndices = find(abs(A*x - b) < epsilon);
	A1 = A(activeIndices, :);
	
	J = [A1; C];
	
	
	if isempty(J)
		% Wenn keine Restriktion aktiv ist,
		% d.h. Ax < b und C = 0, c = 0,
		% dann waehle ein anderes Verfahren.
		% Hier z.B. steilsten Abstieg.
		
		d = -df(x);
	else
		
		%%%%
		% b
		
		% Bestimme den Rang von J
		% Der Rang von J MUSS gleich die Anzahl 
		% der Zeilen von J, um JJ' zu invertieren!
		
		[r, n] = size(J);
		if rank(J) < r
			% Wenn nicht: Negativer Abbruch
			% Alternative: Updating-Formel
			
			error('J hat nicht vollen Rang! Abbruch');
		end
		
		
		
		%%%%
		% c
		
		% Bestimme die Abstiegsrichtung d, indem
		% der Gradient von f auf dem Tangentialraum
		% projiziert wird.
		
		g = -df(x);
		beta = inv(J*J')*J*g;
		d = g - J'*beta;
		
		
		
		%%%%
		% d
		
		% Die Komponenten von beta, die zu A1 gehoert.
		
		beta1 = beta(1:length(activeIndices));
		
		
		% Wenn d != 0 ist, dann springe zu
		% B, das eindimensionale Unterproblem
		
		if (norm(d) < epsilon)
			
			% Zwei moegliche Faelle:
			% 1.) beta1 >= 0, dann ist x KKT-Punkt, Abbruch.
			% 2.) sonst, neues d bestimmen und weiter rechnen.
			
			
			% Finde ein Index l, sodass beta1_l < 0 ist, mit
			% l erfuellt min{ abs(beta1_l) : beta1_l < 0 }
			
			% Die Menge der Indices, so dass beta1_l < 0 ist.
			ind = find(beta1 < 0);
			
			% Bestimme das Maximum.
			% l erfuellt, dann das Maximum.
			% l ist "empty", wenn das Maximum nicht exisiert, d.h.
			% beta1 >= 0.
			[beta1l, l] = max(abs(beta1(ind)));
			
			if (isempty(l))
				% 1.) x ist KKT-Punkt, Abbruch.
				break;
			else
				% 2.) Neues d bestimmen:
				%	Die l-te Zeile, bzw. Gradient, aus J streichen,
				%	dann P neubestimmen und d neubestimmen.
				
				J = J([1:l-1 l+1:r], :);
				d = g - J'*inv(J*J')*J*g;
			end
		end
	end
	
	
	%%
	% B Eindim. Unterproblem
	%%
	
	% Das eindimensionale Unterproblem:
	% min{ f(x + alpha*d) : 0 <= alpha <= alpha^sup }
	
	% alpha^sup bestimmen:
	% d ist eigentliche Abstiegsrichtung, d.h. d zeigt ins M rein.
	%
	% Jd = 0, d.h. d ist orthogonal zu den Ebenen, die
	% die Menge begrenzen und die Restriktionen aktiv sind.
	% D.h. fuer alpha^sup reicht es, 
	% nur die NICHT aktiven Indices zu betrachten.
	
	% Menge der nicht aktiven Indices.
	nonActiveIndices = setdiff(1:m, activeIndices);
	alpha_sup = inf;
	
	for i = nonActiveIndices
		
		% x, d fest
		% Betrachte A(i, :)*(x + alpha*d) <= b(i)
		% glw zu alpha*A(i, :)*d  <=  b(i) - A(i, :)*x
		% Bestimme alpha maximal.
		
		% Der Index i ist nicht aktiv, also ist
		% A(i, :)*x < b(i), also ist
		% 0 < b(i) - A(i, :)*x
		
		nenner = A(i, :)*d;
		
		% Ist der Nenner = 0, dann ist alpha_sup = inf.
		if nenner ~= 0 
			alpha = (b(i) - A(i, :)*x) / nenner;
			if nenner > 0 && (alpha < alpha_sup)
				alpha_sup = alpha;
			else
				% Ist Nenner < 0, dann gilt: 
				% alpha >= (b(i) - A(i, :)*x) / nenner;
				% wobei (b(i) - A(i, :)*x) / nenner < 0, 
				% d.h. alpha ist nach oben unbeschraenkt.
				%
				% Oder alpha >= alpha_sup
			end
		end
	end
	
	
	
	% Loese das eindimensionale Unterproblem
	% min{ f(x + alpha*d) : 0 <= alpha <= alpha^sup }
	
	u = @(alpha) f(x + alpha*d);
	du = @(alpha) d'*df(x + alpha*d);
	
	% Hier Spezialfall: Die Zielfunktion ist quadratisch.
	% Ansonsten Bisektion, Hybrid-Newtow usw. verwenden.

	alpha = minQuadPoly(u, du, 0, alpha_sup);
	
	if (alpha == inf)
		% Negativer Abbruch, wenn das Minimum nicht existiert.
		error('Das eindimensionale Unterproblem ist nicht loesbar.');
	end
	
	
	%%
	% C Neue Iterierte
	%%
	
	x = x + alpha*d;
end