// Author:  Douglas Wilhelm Harder
// Copyright (c) 2009 by Douglas Wilhelm Harder.  All rights reserved.
// Reference:  Yousef Saad, Iterative Methods for Sparse Linear Systems, SIAM, Philadelphia, 2003.

template <int N>
Vector<N, COLUMN> steepest_descent( Matrix<N, N> const &A, Vector<N, COLUMN> const &b, int K, double epsilon ) {
	Vector<N> x = diagonal_solve( A, b );
	Vector<N> r = b - A*x;
	Vector<N> p = A*r;

	for ( int i = 0; i < K; ++i ) {
		double alpha = (transpose(r)*r) / (transpose(p)*r);
		x = x + alpha*r;

		if ( norm( alpha*r ) < epsilon ) {
			return x;
		}

		r = r - alpha*p;
		p = A*r;
	}

	throw non_convergence( K );
}

template <int N>
Vector<N, COLUMN> minimal_residual( Matrix<N, N> const &A, Vector<N, COLUMN> const &b, int K, double epsilon ) {
	Vector<N> x = diagonal_solve( A, b );
	Vector<N> r = b - A*x;
	Vector<N> p = A*r;

	for ( int i = 0; i < K; ++i ) {
		double alpha = (transpose(r)*r) / (transpose(p)*p);
		x = x + alpha*r;

		if ( norm( alpha*r ) < epsilon ) {
			return x;
		}

		r = r - alpha*p;
		p = A*r;
	}

	throw non_convergence( K );
}

template <int N>
Vector<N, COLUMN> residual_norm_steepest_descent( Matrix<N, N> const &A, Vector<N, COLUMN> const &b, int K, double epsilon ) {
	Vector<N> x = diagonal_solve( A, b );
	Vector<N> r = b - A*x;

	for ( int i = 0; i < K; ++i ) {
		// A'*r
		Vector<N> v = transpose( transpose(r)*A );
		Vector<N> Av = A*v;

		double alpha = norm( v, 2 )/norm( Av, 2 );
		alpha *= alpha;

		x = x + alpha*v;

		if ( norm( alpha*v ) < epsilon ) {
			return x;
		}

		r = r - alpha*Av;
	}

	throw non_convergence( K );
}