/*
 *  DSPQRSL_RT Helper function for QR decomposition.
 *
 *  Copyright (c) 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.7 $  $Date: 2000/03/21 21:01:38 $
 */

#include "dspqrsl_rt.h"

/*
 * dspcompqy - compute q*y or q'*y in place over y
 */
void dspcompqy_real (
    int_T	n,
    int_T	j,
    real_T	*qr,
    real_T	*qrauxj,
    real_T	*y
    )
{

    if (fabs(*qrauxj) != 0.0) {
        int_T	nmj, i, pjj;
        real_T	t, temp, *pqr, *py;

	nmj = n - j;
	pjj = j*(n+1); /* offset for qr(j,j) */
	pqr  = qr + pjj;
	temp = *pqr;
	*pqr = *qrauxj;
	t = 0.0;
	py = y;
	for(i=nmj; i-- > 0; ) {
	    t -= (*pqr) * (*py);
	    pqr++;
	    py++;
	}
	pqr = qr + pjj; /* reset to pointer to qr(j,j) */
	t /= *pqr;
	py = y;
	for(i=nmj; i-- > 0; ) {
	    *py += t * (*pqr);
	    pqr++;
	    py++;
	}
	pqr = qr + pjj; /* reset to pointer to qr(j,j) */
	*pqr = temp;
    }
}


void dspcompqy_cplx (
    int_T	n,
    int_T	j,
    creal_T	*qr,
    creal_T	*qrauxj,
    creal_T	*y
    )
{
    if (CQABS(*qrauxj) != 0.0) {
	int_T	nmj, i, pjj;
	creal_T	t, temp, *pqr, *py, Zero = {0.0, 0.0};

	nmj  = n - j;
	pjj  = j*(n+1);	/* offset for x(j,j) */
	pqr   = qr + pjj;	/* pointer to x(j,j) */
	temp = *pqr;
	*pqr  = *qrauxj;
	t = Zero;
	py = y;
	for(i=nmj; i-- > 0; ) {
	    t.re -= CMULT_XCONJ_RE(*pqr, *py);
	    t.im -= CMULT_XCONJ_IM(*pqr, *py);
	    pqr++;
	    py++;
	}
	pqr = qr + pjj;	/* reset to pointer to qr(j,j) */
	CDIV(t, *pqr, t);
	py = y;
	for(i=nmj; i-- > 0; ) {
	    py->re += CMULT_RE(t, *pqr);
	    py->im += CMULT_IM(t, *pqr);
	    pqr++;
	    py++;
	}
	pqr = qr + pjj;	/* reset to pointer to qr(j,j) */
	*pqr = temp;
    }
}


void dspcompqy_mixd(
    int_T	n,
    int_T	j,
    real_T	*qr,
    real_T	*qrauxj,
    creal_T	*y
    )
{
    if (fabs(*qrauxj) != 0.0) {
        int_T	nmj, i, pjj;
        real_T	temp, *pqr;
        creal_T	t, *py, Zero = {0.0, 0.0};
	nmj  = n - j;
	pjj  = j*(n+1);	/* offset for x(j,j) */
	pqr  = qr + pjj;	/* pointer to x(j,j) */
	temp = *pqr;
	*pqr  = *qrauxj;
	t = Zero;
	py = y;
	for(i=nmj; i-- > 0; ) {
	    t.re -= *pqr * py->re;
	    t.im -= *pqr * py->im;
	    pqr++;
	    py++;
	}
	pqr = qr + pjj;	/* reset to pointer to x(j,j) */
	t.re /= *pqr;
	t.im /= *pqr;
	py = y;
	for(i=nmj; i-- > 0; ) {
	    py->re += t.re * *pqr;
	    py->im += t.im * *pqr;
	    pqr++;
	    py++;
	}
	pqr = qr + pjj;	/* reset to pointer to x(j,j) */
	*pqr = temp;
    }
}


/*
 * dspqrsl1 - use the qr factorization stored in qr and qraux
 * to operate on y and compute q*y in place over y
 */
int_T dspqrsl1_real(
    int_T	n,
    int_T	k,
    real_T	*qr,
    real_T	*qraux,
    real_T	*y
    )
{
    int_T	j, info=0;
    real_T	*pqraux, *py;

    j = MIN(k,n-1);

    /* special action when n=1 */
    if (j == 0) {
	return(info);
    }

    /* compute qy */
    pqraux = qraux + j-1;
    py = y + j-1;
    while (j--) {
	dspcompqy_real(n, j, qr, pqraux--, py--);
    }
    return(info);
}


int_T dspqrsl1_cplx(
    int_T		n,
    int_T		k,
    creal_T	*qr,
    creal_T	*qraux,
    creal_T	*y
    )
{
    int_T	j, info=0;
    creal_T	*pqraux, *py;

    j = MIN(k,n-1);

    /* special action when n=1 */
    if (j == 0) {
	return(info);
    }

    /* compute qy */
    pqraux = qraux + j-1;
    py = y + j-1;
    while (j--) {
	    dspcompqy_cplx(n, j, qr, pqraux--, py--);
    }
    return(info);
}


/*
 * dspqrsl2 - use the qr factorization of a stored in qr and qraux
 * to operate on input b and compute q'*b and the solution x to
 * min(norm(b-q*r*x)) = min(norm(q'*b-r*x)) in place over b.
 *
 * a and its qr factorization stored in qr and qraux are real;
 * b is real.
 */
int_T dspqrsl2_real(
    int_T	n,
    int_T	k,
    real_T	*qr,
    real_T	*qraux,
    real_T	*b
    )
{
	int_T	i, j, ju, info=0;
	real_T	t, *pqr, *pqraux, *pb, *pbj;

	ju = MIN(k,n-1);

	/* special action when n=1 */
	if (ju == 0) {
	    if (fabs(*qr) == 0.0) {
		info = 1;
	    }
	    else {
		*b /= *qr;
	    }
	    return(info);
	}

	/* compute q'*b in place over b */
	pqraux = qraux;
	pb = b;
	for (j=0; j<ju; j++) {
	    dspcompqy_real(n, j, qr, pqraux++, pb++);
	}

	/* compute x, solution to min(norm(b-a*x)), in place over b */
	pbj = b + k-1;	/* pointer to b(j) */
	for (j=k-1; j>=0; j--) {
		pqr = qr + j * (n + 1);	/* pointer to qr(j,j) */
		if (fabs(*pqr) == 0.0){
			info = j + 1;
			break;
		}
		*pbj /= *pqr;
		if (j != 0) {
			t = -*pbj;
			pb = b;
			pqr -= j;
			for(i=j; i-- > 0; ) {
				*pb++ += t * *pqr++;
			}
		}
		pbj--;
	}

	return(info);
}


/*
 * a and its qr factorization stored in qr and qraux are complex;
 * b is complex.
 */
int_T dspqrsl2_cplx(
    int_T	n,
    int_T	k,
    creal_T	*qr,
    creal_T	*qraux,
    creal_T	*b
    )
{
	int_T	i, j, ju, info=0;
	creal_T	t, *pqr, *pqraux, *pb, *pbj;

	ju = MIN(k,n-1);

	/* special action when n=1 */
	if (ju == 0) {
		if (CQABS(*qr) == 0.0) {
			info = 1;
		}
		else {
			CDIV(*b, *qr, *b);
		}
		return(info);
	}

	/* compute q'*b in place over b */
	pqraux = qraux;
	pb = b;
	for (j=0; j<ju; j++) {
		dspcompqy_cplx(n, j, qr, pqraux++, pb++);
	}

	/* compute x, solution to min(norm(b-a*x)), in place over b */
	pbj = b + k-1; /* pointer to b(j) */
	for (j=k-1; j>=0; j--) {
		pqr = qr + j * (n + 1);	/* pointer to qr(j,j) */
		if (CQABS(*pqr) == 0.0){
			info = j + 1;
			break;
		}
		CDIV(*pbj, *pqr, *pbj);
		if (j != 0) {
			t.re = -(pbj->re);
			t.im = -(pbj->im);
			pb = b;
			pqr -= j;
			for(i=j; i-- > 0; ) {
				pb->re += CMULT_RE(t, *pqr);
				pb->im += CMULT_IM(t, *pqr);
				pb++;
				pqr++;
			}
		}
		pbj--;
	}

	return(info);
}


/*
 * a and its qr factorization stored in qr and qraux are real;
 * b is complex.
 */
int_T dspqrsl2_mixd(
    int_T	n,
    int_T	k,
    real_T	*qr,
    real_T	*qraux,
    creal_T	*b
    )
{
	int_T	i, j, ju, info=0;
	real_T	*pqr, *pqraux;
	creal_T	t, *pb, *pbj;


	ju = MIN(k,n-1);

	/* special action when n=1 */
	if (ju == 0) {
		if (fabs(*qr) == 0.0) {
			info = 1;
		}
		else {
			b->re /= *qr;
			b->im /= *qr;
		}
		return(info);
	}

	/* compute q'*b in place over b */
	pqraux = qraux;
	pb = b;
	for (j=0; j<ju; j++) {
		dspcompqy_mixd(n, j, qr, pqraux++, pb++);
	}

	/* compute x, solution to min(norm(b-a*x)), in place over b */
	pbj = b + k-1;	/* pointer to b(j) */
	for (j=k-1; j>=0; j--) {
		pqr = qr + j * (n + 1);	/* pointer to qr(j,j) */
		if (fabs(*pqr) == 0.0){
			info = j + 1;
			break;
		}
		pbj->re /= *pqr;
		pbj->im /= *pqr;
		if (j != 0) {
			t.re = -(pbj->re);
			t.im = -(pbj->im);
			pb = b;
			pqr -= j;
			for(i=j; i-- > 0; ) {
				pb->re += t.re * *pqr;
				pb->im += t.im * *pqr;
				pb++;
				pqr++;
			}
		}
		pbj--;
	}

	return(info);
}

/* [EOF] dspqrsl_rt.c */