// Daniel Knüttel Aufgabe2

/*
* Copyright(c) 2018 Daniel Knüttel 
*/

/*    This program is free software: you can redistribute it and/or modify
*    it under the terms of the GNU General Public License as published by
*    the Free Software Foundation, either version 3 of the License, or
*    (at your option) any later version.
*
*    This program is distributed in the hope that it will be useful,
*    but WITHOUT ANY WARRANTY; without even the implied warranty of
*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*    GNU General Public License for more details.
*
*    You should have received a copy of the GNU General Public License
*    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*
*    Dieses Programm ist Freie Software: Sie können es unter den Bedingungen
*    der GNU General Public License, wie von der Free Software Foundation,
*    Version 3 der Lizenz oder (nach Ihrer Wahl) jeder neueren
*    veröffentlichten Version, weiterverbreiten und/oder modifizieren.
*
*    Dieses Programm wird in der Hoffnung, dass es nützlich sein wird, aber
*    OHNE JEDE GEWÄHRLEISTUNG, bereitgestellt; sogar ohne die implizite
*    Gewährleistung der MARKTFÄHIGKEIT oder EIGNUNG FÜR EINEN BESTIMMTEN ZWECK.
*    Siehe die GNU General Public License für weitere Details.
*
*    Sie sollten eine Kopie der GNU General Public License zusammen mit diesem
*    Programm erhalten haben. Wenn nicht, siehe <http://www.gnu.org/licenses/>.
*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define min(a, b) (((a) < (b)) ? (a) : (b))
#define square(a) ((a) * (a))
#define kronecker_delta(i, j) ((i) == (j) ? 1 : 0)

/*
 * \brief Computes the householder partition of A.
 * 		Stores the generating vectors of Q in the memory passed as A,
 * 		the diagonal elements in alpha and the rest of R in the memory 
 * 		passed as A.
 *
 * \param A Matrix to partition. After the partition this memory contains Q and R.
 * \param alpha Will be filled with diagonal elements of R.
 * \param n Length of A[i].
 * \param m Length of A.
 * \return Status.
 * */
int householder(double ** A, double * alpha, int m, int n);

/*
 * \brief Computes the solution of and n times n system using a QR partition.
 * 		Overwrites b.
 *
 * \param A Matrix that has been modified by the function householder.
 * \param alpha Diagonal elements of R, produced by the function householder.
 * \param n Length of A and A[i].
 * \param b Vector b for Ax=b. Will be overwritten with elements of x.
 * \return Status.
 * */
int solve_QR(double ** A, double * alpha, int n, double * b);

/*
 * \brief Computes Q^Tb and stores the result in b.
 *
 * \param A Matrix that has been modified by the function householder.
 * \param alpha Diagonal elements of R, produced by the function householder.
 * \param n Length of A and A[i].
 * \param b Vector b for Ax=b. Will be overwritten with elements of the result.
 *
 * \return Status.
 * */
int qtb(double ** A, double * alpha, int n, double * b);

/*
 *
 * \brief Uses backwards substitution to solve R = Q^Tb. b := Q^Tb has already been calculated.
 *
 * \param A Matrix that has been modified by the function householder.
 * \param alpha Diagonal elements of R, produced by the function householder.
 * \param n Length of A and A[i].
 * \param b Vector b for Ax=b. Will be overwritten with elements of the result.
 *
 * \return Status.
 *
 * */
int rw_subs(double ** A, double * alpha, int n, double * b);


/*
 * \brief Print the matrix in a nicely formatted way to stream.
 *
 * \param stream The output stream.
 * \param matrix The matrix to print.
 * \param n Length of A[i].
 * \param m Length of A.
 *
 * */
int fprintm(FILE * stream, double ** matrix, int m, int n);
/*
 * \brief Print the vector in a nicely formatted way to stream.
 *
 * \param stream The output stream.
 * \param vector The vector to print.
 * \param n Length of vector.
 *
 * */
int fprintv(FILE * stream, double * vector, int n);

/*
 * \brief Extract R from A and alpha, allocates R dynamically.
 *
 * \param A Marix computed by householder.
 * \param alpha Vector computed by householder.
 * \param m Length of A.
 * \param n Length of A[i].
 * \param R Pointer to the result (unallocated).
 * */
int unwind_R(double ** A, double * alpha, int m, int n, double *** R);
/*
 * \brief Extract v from A and alpha, allocates R dynamically.
 *
 * \param A Marix computed by householder.
 * \param alpha Vector computed by householder.
 * \param m Length of A.
 * \param n Length of A[i].
 * \param v Pointer to the results (unallocated).
 * */
int unwind_v(double ** A, int m, int n, double *** v);

#define printm(matrix, n, m) fprintm(stdout, matrix, n, m)
#define printv(vector, n) fprintv(stdout, vector, n)

int main(void)
{
	int status = 0;
	int i, j;
	int m, n;

	// PART a: QR-Partition.
	
	double A_stack_1[15] = {2, 2.23606797749979, 0
				, 2, 3, -1
				, 2, 3, -1
				, 1, 0, -1
				, 0, 2.6457513110645907, 5.669467095138408
				};
	m = 5;
	n = 3;
	double ** A_1 = malloc(sizeof(double *) * m);
	for(i = 0; i < m; i++)
	{
		A_1[i] = malloc(sizeof(double) * n);
		for(j = 0; j < n; j++)
		{
			A_1[i][j] = A_stack_1[n*i + j];
		}
	}
	printf("A:\n");
	printm(A_1, m, n);

	double * alpha_1 = malloc(sizeof(double) * m);

	status = householder(A_1, alpha_1, m, n);
	if(status)
	{
		fprintf(stderr, "failed to compute QR decomposition\n");
		goto exit;
	}

	double ** R_1, ** vs_1;

	status = unwind_R(A_1, alpha_1, m, n, &R_1);
	if(status)
	{
		fprintf(stderr, "failed to unwind R\n");
		goto exit;
	}

	status = unwind_v(A_1, m, n, &vs_1);
	if(status)
	{
		fprintf(stderr, "failed to unwind v\n");
		for(i = 0; i < m; i++)
		{
			free(R_1[i]);
		}
		free(R_1);
		goto exit;
	}

	printf("R:\n");
	printm(R_1, m, n);

	printf("Generating vectors of Q:\n");
	for(i = 0; i < n; i++)
	{
		printv(vs_1[i], m);
		free(vs_1[i]);
	}

	free(vs_1);
	for(i = 0; i < m; i++)
	{
		free(R_1[i]);
	}
	free(R_1);

	free(alpha_1);
	for(i = 0; i < m; i++)
	{
		free(A_1[i]);
	}
	free(A_1);



	double A_stack_2[16] = {2, -1, 3, 0
				, 7, 0, 1.4142135623730951, 1
				, 1, 1, 1, 1
				, 0, 10, 3, 2
				};

	m = 4;
	n = 4;

	double ** A_2 = malloc(sizeof(double *) * m);
	for(i = 0; i < m; i++)
	{
		A_2[i] = malloc(sizeof(double) * n);
		for(j = 0; j < n; j++)
		{
			A_2[i][j] = A_stack_2[n*i + j];
		}
	}

	double * b = malloc(sizeof(double) * m);
	double b_stack[4] = {1, 0, 0, 0};

	for(i = 0; i < m; i++)
	{
		b[i] = b_stack[i];
	}

	printf("System: A, b\n");
	printm(A_2, m, m);
	printv(b, m);


	double * alpha_2 = malloc(sizeof(double) * m);

	status = householder(A_2, alpha_2, m, n);
	if(status)
	{
		fprintf(stderr, "failed to compute QR decomposition\n");
		goto exit;
	}

	status = solve_QR(A_2, alpha_2, m, b);
	printf("Solution for the System:\n");
	printv(b, m);

	free(alpha_2);
	for(i = 0; i < m; i++)
	{
		free(A_2[i]);
	}
	free(A_2);
	free(b);


	

exit:
	if(status)
	{
		fprintf(stderr, "something went wrong\n");
	}
	return status;
}


int householder(double ** A, double * alpha, int m, int n)
{
	if(n > m)
	{
		return 1;
	}
	int k, i, j;
	double beta, gamma, delta;
	/*
	 * This is just the sample implementation from the 
	 * lecture script.
	 * */
	for(k = 0; k < min(n, m - 1); k++)
	{
		alpha[k] = square(A[k][k]); 

		for(j = k + 1; j < m; j++)
		{
			alpha[k] += square(A[j][k]);
		}

		alpha[k] = sqrt(alpha[k]);
		if(A[k][k] < 0)
		{
			alpha[k] *= -1;
		}
		
		beta = alpha[k] * (A[k][k] - alpha[k]);
		A[k][k] -= alpha[k];

		for(i = k + 1; i < n ; i++)
		{
			gamma = A[k][k] * A[k][i];

			for(j = k + 1; j < m; j++)
			{
				gamma += A[j][k] * A[j][i];
			}
			
			delta = gamma / beta;

			for(j = k; j < m; j++)
			{
				A[j][i] += delta * A[j][k];
			}
		}


	}
	return 0;
}

int fprintm(FILE * stream, double ** matrix, int m, int n)
{
	int i, j, status = 0;

	for(i = 0; i < m; i++)
	{
		if(i == 0)
		{
			fprintf(stream, "T ");
		} 
		else if(i == m - 1)
		{
			fprintf(stream, "L ");
		}
		else
		{
			fprintf(stream, "| ");
		}

		for(j = 0; j < n; j++)
		{
			fprintf(stream, "%6.2f ", matrix[i][j]);
		}

		if(i == 0)
		{
			fprintf(stream, "T\n");
		} 
		else if(i == m - 1)
		{
			fprintf(stream, "J\n");
		}
		else
		{
			fprintf(stream, "|\n");
		}
	}
	fflush(stream);
	return status;
}

int fprintv(FILE * stream, double * vector, int n)
{
	int i;

	for(i = 0; i < n; i++)
	{
		if(i == 0)
		{
			fprintf(stream, "T ");
		} 
		else if(i == n - 1)
		{
			fprintf(stream, "L ");
		}
		else
		{
			fprintf(stream, "| ");
		}

		fprintf(stream, "%6.2f ", vector[i]);

		if(i == 0)
		{
			fprintf(stream, "T\n");
		} 
		else if(i == n - 1)
		{
			fprintf(stream, "J\n");
		}
		else
		{
			fprintf(stream, "|\n");
		}
		
	}

	fflush(stream);
	return 0;
}

int qtb(double ** A, double * alpha, int n, double * b)
{
	/*
	 * Some short computing gives that:
	 *
	 * Q[i][j] = kronecker_delta(i, j) - 2*v[i]v[j]
	 * and for
	 * Q^T b = x
	 * x[i] = sum(over j)(Q[j][i]*b[j])
	 * */

	int i, j;
	double x;
	for(i = 0; i < n; i++)
	{
		for(j = 0; j < n; j++)
		{
			x += b[j] * (kronecker_delta(i, j)
					- 2 * A[i][i] * A[i][j]);
		}
		b[i] = x;
	}
	return 0;
}


int rw_subs(double ** A, double * alpha, int n, double * b)
{

	/*
	 * In the lecture section 1 about the gaussian algorithm 
	 * we have given the following formula: 
	 *
	 * x[j] = 1/A[j][j] * (b[j] - sum(from j + 1 over k to n)(a[j][k]*x[k]))
	 *
	 * So loop from k = n to 1 to avoid recursion and 
	 * substitute the content of b with the solution.
	 * */
	int j, k;
	double tmp;
	// Note: indices start at 0.
	for(j = n - 1; j >= 0; j--)
	{
		// Note: we start from k + 1, so there are no
		// diagonal elements of A contained.
		for(k = j + 1; k < n; k++)
		{
			tmp += b[k] * A[j][k];
		}

		// A[j][j] = alpha[j].
		b[j] = (b[j] - tmp) / alpha[j];

	}
	return 0;
}

int solve_QR(double ** A, double * alpha, int n, double * b)
{
	if(qtb(A, alpha, n, b))
	{
		return 1;
	}
	if(rw_subs(A, alpha, n, b))
	{
		return 1;
	}
	return 0;
}

int unwind_R(double ** A, double * alpha, int m, int n, double *** R)
{
	double ** result = malloc(sizeof(double *) * m);
	int i, j;
	int have_to_free_all = 0;
	if(!result)
	{
		return 1;
	}

	// Allocate the memory.
	for(i = 0; i < m; i++)
	{
		result[i] = malloc(sizeof(double) * n);
		if(!result[i])
		{
			have_to_free_all = 1;
			break;
		}
	}

	// Out of Memory.
	if(have_to_free_all)
	{
		for(j = 0; j < i; j++)
		{
			free(result[i]);
		}
		free(result);
		return 1;
	}

	// Alloc is OK.
	// Calculate the result.
	for(i = 0; i < m; i++)
	{
		for(j = 0; j < n; j++)
		{
			if(i > j)
			{
				result[i][j] = 0;
			}
			if(i == j)
			{
				result[i][j] = alpha[j];
			}
			if(i < j)
			{
				result[i][j] = A[i][j];
			}
		}
	}
	*R = result;
	return 0;

}


int unwind_v(double ** A, int m, int n, double *** v)
{
	// We have n vectors.
	double ** result = malloc(sizeof(double *) * n);
	int i, j;
	int have_to_free_all = 0;
	if(!result)
	{
		return 1;
	}

	// Allocate the memory.
	for(i = 0; i < n; i++)
	{
		// They are m long.
		result[i] = malloc(sizeof(double) * m);
		if(!result[i])
		{
			have_to_free_all = 1;
			break;
		}
	}

	// Out of Memory.
	if(have_to_free_all)
	{
		for(j = 0; j < i; j++)
		{
			free(result[i]);
		}
		free(result);
		return 1;
	}

	// Alloc is OK.
	// Calculate the result.
	for(i = 0; i < m; i++)
	{
		for(j = 0; j < n; j++)
		{
			if(i > j)
			{
				result[j][i] = 0;
			}
			else
			{
				result[j][i] = A[j][i];
			}
		}
	}
	*v = result;
	return 0;

}