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

/***************************************************
 * Matrix-Matrix Addition
 * A = A + B
 *
 * Run time:   O(M + na + nb)
 * Memory:     O(na + nb)
 ***************************************************/

template <int M, int N>
Matrix<M, N> &Matrix<M, N>::operator+= ( Matrix<M, N> const &B ) {
	if ( this == &B ) {
		*this *= 2.0;
		return *this;

	}

	for ( int i = 0; i < minMN; ++i ) {
		diagonal[i] += B.diagonal[i];
	}

	int A_row_index[M + 1];

	for ( int i = 0; i <= M; ++i ) {
		A_row_index[i] = row_index[i];
	}

	int capacity = count_nonzero_entries( *this, B, true );
	int    *A_column_index = column_index;
	double *A_off_diagonal = off_diagonal;

	column_index = new int[capacity];
	off_diagonal = new double[capacity];

	// Adding the off-diagonal entries is a process similar to
	// merging two sorted lists; however, there are some differences:
	//
	//  - We merge each of the rows separately and in order
	//  - For each row, we merge on the column indices
	//     - If the column indices match, sum the corresponding
	//       entries and add them only if the sum is non-zero;
	//     - Otherwise, if there is only one non-zero entry
	//       with one column, just add it

	int  j = 0;
	int aj = 0;
	int bj = 0;

	for ( int i = 1; i <= M; ++i ) {
		// Step through the ith row of both matrices B and B
		// so long as there are still entries which have not
		// been inserted into C

		while ( aj < A_row_index[i] && bj < B.row_index[i] ) {
			// If the next entries in row i of both B and B have
			// the same column index, update C(i, j) =  B(i, j) + B(i, j)
			// if the sum is non-zero;
			// Otherwise, if the next non-zero entry of B comes before
			// the next non-zero entry of B, update C(i, j) = B(i, j);
			// Otherwise update C(i, j) = B(i, j).

			if ( A_column_index[aj] == B.column_index[bj] ) {
				double sum = A_off_diagonal[aj] + B.off_diagonal[bj];

				if ( sum != 0 ) {
					column_index[j] = column_index[aj];
					off_diagonal[j] = sum;
					++j;
				}

				++aj;
				++bj;
			} else if ( A_column_index[aj] < B.column_index[bj] ) {
				column_index[j] = A_column_index[aj];
				off_diagonal[j] = A_off_diagonal[aj];
				++aj;
				++j;
			} else {
				column_index[j] = B.column_index[bj];
				off_diagonal[j] = B.off_diagonal[bj];
				++bj;
				++j;
			}
		}

		// At this point, all entries of row i of either B or B (or both)
		// have added to C
		// Any remaining entries in row i must now be moved over

		while ( aj < A_row_index[i] ) {
			column_index[j] = A_column_index[aj];
			off_diagonal[j] = A_off_diagonal[aj];
			++j;
			++aj;
		}

		while ( bj < B.row_index[i] ) {
			column_index[j] = B.column_index[bj];
			off_diagonal[j] = B.off_diagonal[bj];
			++j;
			++bj;
		}

		// At this point, we are now guaranteed that all entries
		// of row i are contained in the entries up to j

		row_index[i] = j;
	}

	delete [] A_column_index;
	delete [] A_off_diagonal;

	return *this;
}

/***************************************************
 * Matrix-Matrix Subtraction
 * A = A - B
 *
 * Run time:   O(M + na + nb)
 * Memory:     O(na + nb)
 ***************************************************/

template <int M, int N>
Matrix<M, N> &Matrix<M, N>::operator-= ( Matrix<M, N> const &B ) {
	if ( this == &B ) {
		return *this *= 2.0;
	}

	for ( int i = 0; i < minMN; ++i ) {
		diagonal[i] -= B.diagonal[i];
	}

	int A_row_index[M + 1];

	for ( int i = 0; i <= M; ++i ) {
		A_row_index[i] = row_index[i];
	}

	int capacity = count_nonzero_entries( *this, B, false );
	int    *A_column_index = column_index;
	double *A_off_diagonal = off_diagonal;

	column_index = new int[capacity];
	off_diagonal = new double[capacity];

	// Adding the off-diagonal entries is a process similar to
	// merging two sorted lists; however, there are some differences:
	//
	//  - We merge each of the rows separately and in order
	//  - For each row, we merge on the column indices
	//     - If the column indices match, difference the corresponding
	//       entries and add them only if the difference is non-zero;
	//     - Otherwise, if there is only one non-zero entry
	//       with one column, just add it

	int  j = 0;
	int aj = 0;
	int bj = 0;

	for ( int i = 1; i <= M; ++i ) {
		// Step through the ith row of both matrices B and B
		// so long as there are still entries which have not
		// been inserted into C

		while ( aj < A_row_index[i] && bj < B.row_index[i] ) {
			// If the next entries in row i of both B and B have
			// the same column index, update C(i, j) =  B(i, j) + B(i, j)
			// if the difference is non-zero;
			// Otherwise, if the next non-zero entry of B comes before
			// the next non-zero entry of B, update C(i, j) = B(i, j);
			// Otherwise update C(i, j) = B(i, j).

			if ( A_column_index[aj] == B.column_index[bj] ) {
				double difference = A_off_diagonal[aj] - B.off_diagonal[bj];

				if ( difference != 0 ) {
					column_index[j] = A_column_index[aj];
					off_diagonal[j] = difference;
					++j;
				}

				++aj;
				++bj;
			} else if ( A_column_index[aj] < B.column_index[bj] ) {
				column_index[j] = A_column_index[aj];
				off_diagonal[j] = A_off_diagonal[aj];
				++aj;
				++j;
			} else {
				column_index[j] =  B.column_index[bj];
				off_diagonal[j] = -B.off_diagonal[bj];
				++bj;
				++j;
			}
		}

		// At this point, all entries of row i of either B or B (or both)
		// have added to C
		// Any remaining entries in row i must now be moved over

		while ( aj < A_row_index[i] ) {
			column_index[j] = A_column_index[aj];
			off_diagonal[j] = A_off_diagonal[aj];
			++j;
			++aj;
		}

		while ( bj < B.row_index[i] ) {
			column_index[j] =  B.column_index[bj];
			off_diagonal[j] = -B.off_diagonal[bj];
			++j;
			++bj;
		}

		// At this point, we are now guaranteed that all entries
		// of row i are contained in the entries up to j

		row_index[i] = j;
	}

	delete [] A_column_index;
	delete [] A_off_diagonal;

	return *this;
}

/***************************************************
 * *********************************************** *
 * *                                             * *
 * *   FRIENDS                                   * *
 * *                                             * *
 * *********************************************** *
 ***************************************************/

/***************************************************
 * Calculate C = A + B
 *
 * Run time:   O( M + an + bn )
 * Memory:     O( M + an + bn )
 ***************************************************/

template <int M, int N>
Matrix<M, N> operator+ ( Matrix<M, N> const &A, Matrix<M, N> const &B ) {
	Matrix<M, N> C( A );

	C += B;

	return C;
}

/***************************************************
 * Calculate C = A - B
 *
 * Run time:   O( M + an + bn )
 * Memory:     O( M + an + bn )
 ***************************************************/

template <int M, int N>
Matrix<M, N> operator- ( Matrix<M, N> const &A, Matrix<M, N> const &B ) {
	Matrix<M, N> C( A );

	C -= B;

	return C;
}