// Author:  Douglas Wilhelm Harder
// Copyright (c) 2009 by Douglas Wilhelm Harder.  All rights reserved.

/*******************************************************
 * *************************************************** *
 * *                                                 * *
 * *  Solving systems of Linear Equations            * *
 * *                                                 * *
 * *  Iterative:                                     * *
 * *      The Jacobi Method                          * *
 * *      The Gauss-Seidel Method                    * *
 * *  Direct:                                        * *
 * *    All direct methods assume that the           * *
 * *    matrix has a particular shape.               * *
 * *       Lower Triangular (backward substitution)  * *
 * *       Upper Triangular (forward substitution)   * *
 * *       Diagonal (evaluation)                     * *
 * *                                                 * *
 * *************************************************** *
 *******************************************************/


/***************************************************
 * Solve the system Av = b by using the Jacobi
 * method.
 *
 * Run time:   O( k(N + na) )
 * Memory:     O( N )
 ***************************************************/

template <int N>
Vector<N, COLUMN> jacobi( Matrix<N, N> const &A, Vector<N, COLUMN> const &b, int K, double epsilon ) {
	Vector<N, COLUMN> v = diagonal_solve( A, b );
	Vector<N, COLUMN> v0;

	for ( int k = 0; k < K; ++k ) {
		for ( int i = 0; i < N; ++i ) {
			v0.array[i] = v.array[i];
		}

		for ( int i = 0; i < N; ++i ) {
			v.array[i] = b.array[i];

			for ( int j = A.row_index[i]; j < A.row_index[i + 1]; ++j ) {
				v.array[i] -= A.off_diagonal[j] * v0.array[A.column_index[j]];
			}

			v.array[i] /= A.diagonal[i];
		}

		v0 -= v;

		if ( v0.norm() < epsilon ) {
			return v;
		}
	}

	throw non_convergence( K );
}

/***************************************************
 * Solve the system Av = b by using the Gauss-
 * Seidel method.
 *
 * Run time:   O( k(N + na) )
 * Memory:     O( N )
 ***************************************************/

template <int N>
Vector<N, COLUMN> gauss_seidel( Matrix<N, N> const &A, Vector<N, COLUMN> const &b, int K, double epsilon ) {
	Vector<N, COLUMN> v = diagonal_solve( A, b );
	Vector<N, COLUMN> v0;

	for ( int k = 0; k < K; ++k ) {
		for ( int i = 0; i < N; ++i ) {
			v0.array[i] = v.array[i];
		}

		for ( int i = 0; i < N; ++i ) {
			v.array[i] = b.array[i];

			for ( int j = A.row_index[i]; j < A.row_index[i + 1]; ++j ) {
				v.array[i] -= A.off_diagonal[j] * v.array[A.column_index[j]];
			}

			v.array[i] /= A.diagonal[i];
		}

		v0 -= v;

		if ( v0.norm() < epsilon ) {
			return v;
		}
	}

	throw non_convergence( K );
}

/***************************************************
 * Solve Av = b where A is assumed to be a
 * diagonal matrix and therefore simple
 * evaluation may be used.
 *  - Entries off the diagonal are ignored.
 *
 * Run time:   O( N )
 * Memory:     O( N )
 ***************************************************/

template <int N>
Vector<N, COLUMN> diagonal_solve( Matrix<N, N> const &A, Vector<N, COLUMN> const &b ) {
	Vector<N, COLUMN> v;

	for ( int i = 0; i < N; ++i ) {
		v.array[i] = b.array[i]/A.diagonal[i];
	}

	return v;
}

/***************************************************
 * Solve Av = b where A is assumed to be
 * upper triangular and therefore backward
 * substitution may be used.
 *  - Entries below the diagonal are ignored.
 *  - If the diagonal entries are 1, pass
 *    identity = true
 *
 * Run time:   O( N + na )
 * Memory:     O( N )
 ***************************************************/

template <int N>
Vector<N, COLUMN> backward_substitution( Matrix<N, N> const &A, Vector<N, COLUMN> const &b, bool identity = false ) {
	Vector<N, COLUMN> v;

	for ( int i = N - 1; i >= 0; --i ) {
		v.array[i] = b.array[i];

		for ( int j = A.row_index[i]; j < A.row_index[i + 1]; ++j ) {
			v.array[i] -= A.off_diagonal[j]*v.array[A.column_index[j]];
		}

		if ( !identity ) {
			v.array[i] /= A.diagonal[i];
		}
	}

	return v;
}

/***************************************************
 * Solve Av = b where A is assumed to be
 * lower triangular and therefore forward
 * substitution may be used.
 *  - Entries above the diagonal are ignored.
 *  - If the diagonal entries are 1, pass
 *    identity = true
 *
 * Run time:   O( N + na )
 * Memory:     O( N )
 ***************************************************/

template <int N>
Vector<N, COLUMN> forward_substitution( Matrix<N, N> const &A, Vector<N, COLUMN> const &b, bool identity = false ) {
	Vector<N, COLUMN> v;

	for ( int i = 0; i < N; ++i ) {
		v.array[i] = b.array[i];

		for ( int j = A.row_index[i]; j < A.row_index[i + 1]; ++j ) {
			v.array[i] -= A.off_diagonal[j]*v.array[A.column_index[j]];
		}

		if ( !identity ) {
			v.array[i] /= A.diagonal[i];
		}
	}

	return v;
}