/**************************************************************

qrdecomp.c (C-Munipack project)
Copyright (C) 2016 David Motl, dmotl@volny.cz

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 2
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, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

**************************************************************/

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

#include "cmpack_common.h"
#include "qrdecomp.h"

/******************      Reflections   ***********************/

/*
 * Generation of an elementary reflection transformation
 */
static void generatereflection(double *x, int n, double *tau)
{
    int j;
    double alpha, xnorm, v, y, z, beta, mx;

    /* Executable Statements .. */
    if (n<=1) {
        *tau = 0.0;
        return;
    }
    
    /* XNORM = DNRM2( N-1, X, INCX ) */
    alpha = x[0];
    mx = 0;
    for (j=1; j<n; j++) {
		y = fabs(x[j]);
		if (y>mx)
			mx = y;
    }

    xnorm = 0;
    if (mx>0) {
		for (j=1; j<n; j++) {
			y = x[j]/mx;
            xnorm = xnorm + (y*y);
		}
        xnorm = sqrt(xnorm)*mx;
    }

    if( xnorm==0 ) {
        /* H  =  I */
        *tau = 0.0;
        return;
    }
    
    /* general case */
	y = fabs(alpha);
	z = fabs(xnorm);
	mx = (y>z ? y : z);
	y /= mx;
	z /= mx;
    beta = -mx*sqrt(y*y+z*z);
    if (alpha<0)
        beta = -beta;
    *tau = (beta-alpha)/beta;

    v = 1.0/(alpha-beta);
	for (j=1; j<n; j++)
		x[j] *= v;
    x[0] = beta;
}

/*
 * Application of an elementary reflection to a rectangular matrix of size MxN
 *
 * The algorithm pre-multiplies the matrix by an elementary reflection transformation
 * which is given by column V and scalar Tau (see the description of the
 * GenerateReflection procedure). Not the whole matrix but only a part of it
 * is transformed (rows from M1 to M2, columns from N1 to N2). Only the elements
 * of this submatrix are changed.
 */
static void applyreflectionfromtheleft(double *c, int m, int n, double tau, const double *v, 
	int m1, int m2, int n1, int n2, double *work)
{
    double t;
    int i, j;

    if (tau==0.0 || n1>n2 || m1>m2)
        return;
    
	memset(work, 0, n*sizeof(double));

    /* w := C' * v */
    for (i=m1; i<=m2; i++) {
        t = v[i-m1];
		for (j=n1; j<=n2; j++)
			work[j] += t * c[j+i*n];
    }
    /* C := C - tau * v * w' */
    for (i=m1; i<=m2; i++) {
        t = v[i-m1] * tau;
		for (j=n1; j<=n2; j++)
			c[j+i*n] -= t * work[j];
    }
}


/*
 * Allocate buffers
 */
void cmpack_qrd_alloc(QRDecomposition *qr, int rows, int cols)
{
	qr->rows = rows;
	qr->cols = cols;
	qr->minmn = (cols<rows ? cols : rows);
	qr->a = (double*) cmpack_malloc((cols*rows)*sizeof(double));
	qr->work = (double*) cmpack_malloc(cols*sizeof(double));
	qr->t = (double*) cmpack_malloc(rows*sizeof(double));
	qr->tau = (double*) cmpack_malloc(qr->minmn*sizeof(double));
}

/* 
 * QR decomposition of matrix A 
 */
void cmpack_qrd_set(QRDecomposition *qr, const double *a, double *q, double *r)
{
	int i, j, k;
	double tmp;

	assert(qr->rows > 0 && qr->cols > 0);

	/* Copy matrix A to temporary matrix a */
	for (j=0; j<qr->rows; j++) {
		for (i=0; i<qr->cols; i++) 
			qr->a[i+j*qr->cols] = a[i+j*qr->cols];
	}

#ifdef DEBUG_OUTPUTS
	/* Print matrix A */
	printf("Matrix A:\n");
	for (j=0; j<qr->rows; j++) {
		for (i=0; i<qr->cols; i++) 
			printf("%10.2e ", qr->a[i+j*qr->cols]);
		printf("\n");
	}
	printf("\n");
#endif

	/* Decomposition, result is in packed form */
    for (i=0; i<qr->minmn; i++) {
        /* Generate elementary reflector H(i) to annihilate A(i+1:m,i) */
		for (j=i, k=0; j<qr->rows; j++, k++)
			qr->t[k] = qr->a[i+j*qr->cols];
        generatereflection(qr->t, qr->rows-i, &tmp);
        qr->tau[i] = tmp;
		for (j=i, k=0; j<qr->rows; j++, k++)
			qr->a[i+j*qr->cols] = qr->t[k];
        qr->t[0] = 1.0;
        if (i<qr->cols) {
            /* Apply H(i) to A(i:m-1,i+1:n-1) from the left */
            applyreflectionfromtheleft(qr->a, qr->rows, qr->cols, qr->tau[i], qr->t, i, qr->rows-1, i+1, qr->cols-1, qr->work);
        }
    }
#ifdef DEBUG_OUTPUTS
	printf("Matrix C:\n");
	for (j=0; j<qr->rows; j++) {
		for (i=0; i<qr->cols; i++)
			printf("%11.2e ", qr->a[i+j*qr->cols]);
		printf("\n");
	}
	printf("\n");
	printf("Vector Tau:\n");
	for (i=0; i<qr->rows && i<qr->cols; i++) {
		printf("%11.2e ", qr->tau[i]);
	}
	printf("\n\n");
#endif

	/* Unpack matrix Q */
	memset(q, 0, (qr->rows*qr->rows)*sizeof(double));
    for (i=0; i<qr->rows; i++)
		q[i+i*qr->rows] = 1.0;

    for (i=qr->minmn-1; i>=0; i--) {
        /* Apply H(i) */
		for (j=i, k=0; j<=qr->rows-1; j++, k++)
			qr->t[k] = qr->a[i+j*qr->cols];
        qr->t[0] = 1.0;
		applyreflectionfromtheleft(q, qr->rows, qr->rows, qr->tau[i], qr->t, i, qr->rows-1, 0, qr->rows-1, qr->work);
    }
#ifdef DEBUG_OUTPUTS
	printf("Matrix Q:\n");
	for (j=0; j<qr->rows; j++) {
		for (i=0; i<qr->rows; i++)
			printf("%11.2e ", q[i+j*qr->rows]);
		printf("\n");
	}
	printf("\n");
#endif

	/* Unpack matrix R */
	memset(r, 0, qr->cols*qr->rows*sizeof(double));
	for (i=0; i<qr->minmn; i++) {
		for (j=i; j<qr->cols; j++)
			r[j+i*qr->cols] = qr->a[j+i*qr->cols];
	}
#ifdef DEBUG_OUTPUTS
	printf("Matrix R:\n");
	for (j=0; j<qr->rows; j++) {
		for (i=0; i<qr->cols; i++)
			printf("%11.2e ", r[i+j*qr->cols]);
		printf("\n");
	}
	printf("\n");

	/* Check results */
	printf("Check result A = QR:\n");
	for (j=0; j<qr->rows; j++) {
		for (i=0; i<qr->cols; i++) {
			double aux = 0.0;
			for (k=0; k<qr->rows; k++) 
				aux += q[k+j*qr->rows] * r[i+k*qr->cols];
			printf("%11.2e ", aux);
		}
		printf("\n");
	}
	printf("Check result I = Q'Q:\n");
	for (j=0; j<qr->rows; j++) {
		for (i=0; i<qr->rows; i++) {
			double aux = 0.0;
			for (k=0; k<qr->rows; k++) 
				aux += q[j+k*qr->rows] * q[i+k*qr->rows];
			printf("%11.2e ", aux);
		}
		printf("\n");
	}
	printf("\n");
#endif
}

/* 
 * Free allocated memory
 */
void cmpack_qrd_free(QRDecomposition *qr)
{
	cmpack_free(qr->a);
	cmpack_free(qr->work);
	cmpack_free(qr->t);
	cmpack_free(qr->tau);
	memset(qr, 0, sizeof(QRDecomposition));
}
