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

void printMatrix(const char, const int, const int, const double*, const int);

int main(){
	int ierr = 0;

	int m = 10, k = 10;
	int n = 2;
	int nb = k;

	double *a = NULL, *b = NULL, *a_concat_b = NULL;
	double *t = NULL;
	double *c = NULL, *d = NULL, *copy_c = NULL;

	a = (double*) malloc(m * k * sizeof(double));
	b = (double*) malloc(m * k * sizeof(double));
	a_concat_b = (double*) malloc( 2 * m * k * sizeof(double));
	t = (double*) malloc(nb * k * sizeof(double));
	c = (double*) malloc(2 * m * n * sizeof(double));
	copy_c = (double*) malloc(2 * m * n * sizeof(double));
	if(a == NULL || b == NULL || a_concat_b == NULL || t == NULL || c == NULL || copy_c == NULL){
		fprintf(stderr, "memory allocation fails");
		exit(-99);
	}
	d = c + m * n;

	srand(1);
	for(int j = 0; j < k; ++j){
		for( int i = 0; i < m; ++i){
			a[i * k + j] = (i > j) ? 0 : ((double)rand())/RAND_MAX;
			b[i * k + j] = (i > j) ? 0 : ((double)rand())/RAND_MAX;
			a_concat_b[i * k + j] = a[i * k + j];
			a_concat_b[m * k + i * k + j] = b[i * k + j];
		}
	}
	memset(t, 0, nb * k * sizeof(double));
	memset(copy_c, 0, 2 * m * n * sizeof(double));
	for(int j = 0; j < n; ++j){
		for( int i = 0; i < m; ++i){
			c[i * n + j] = (i != j) ? 0.0 : 1.0;
			d[i * n + j] = 0.0;
			copy_c[i * n + j] = (i != j) ? 0.0 : 1.0;
		}
	}

	printf("a\n");
	printMatrix('R', m, k, a, k);
	printMatrix('R', m, k, b, k);

	printf("[a;b]\n");
	printMatrix('R', 2 * m, k, a_concat_b, k);

	// QR of the matrix formed by concatenating a and b
	ierr = LAPACKE_dtpqrt(LAPACK_ROW_MAJOR, m, k, k, nb, a, k, b, k, t, k);
	if(ierr){
		fprintf(stderr, "LAPACKE_dtpqrt:: Error in argument %d\n", -ierr);
	}
	// Apply Q factor on the matrix formed by concatenating c and d
	ierr = LAPACKE_dtpmqrt(LAPACK_ROW_MAJOR, 'L', 'N', k, n, k, k, nb, b, k, t, k, c, n, d, n);
	if(ierr){
		fprintf(stderr, "LAPACKE_dtpmqrt:: Error in argument %d\n", -ierr);
	}

	// QR of [a; b]
	ierr = LAPACKE_dgeqrf(LAPACK_ROW_MAJOR , 2 * m, k, a_concat_b, k, t);
	if(ierr){
		fprintf(stderr, "LAPACKE_dgeqrf:: Error in argument %d\n", -ierr);
	}

	// Apply Q on copy_c
	ierr = LAPACKE_dormqr(LAPACK_ROW_MAJOR, 'L', 'N', 2 * m, n, k, a_concat_b, k, t, copy_c, n);
	if(ierr){
		fprintf(stderr, "LAPACKE_dormqr:: Error in argument %d\n", -ierr);
	}
	printf("c\n");
	printMatrix('R', 2 * m, n, c, n);

	printf("copy_c\n");
	printMatrix('R', 2 * m, n, copy_c, n);

	for(int i = 0; i < 2 * m; ++i){
		for(int j = 0; j < n; ++j){
			copy_c[i * n + j] -= c[i * n + j];
		}
	}
	printf("copy_c - c\n");
	printMatrix('R', 2 * m, n, copy_c, n);

	ierr = LAPACKE_dorgqr(LAPACK_ROW_MAJOR, 2 * m, k, k, a_concat_b, k, t);
	if(ierr){
		fprintf(stderr, "LAPACKE_dorgqr:: Error in argument %d\n", -ierr);
	}
	printf("Q\n");
	printMatrix('R', 2 * m, n, a_concat_b, k);

	free(a);
	free(b);
	free(c);
	free(a_concat_b);
	free(copy_c);
	free(t);
	return 0;
}

void printMatrix(const char layout, const int m, const int n, const double* a, const int lda){
  for(int i = 0; i < m; ++i){
    for(int j = 0; j < n; ++j){
			if(layout == 'C'){
				printf("%f\t ", a[i + j * lda]);
			}else{
				printf("%f\t ", a[i * lda + j]);
			}
    }
    printf("\n");
  }
}