#include "cve.h"

// /**
//  * Performas a QR factorization and computes the Q factor.
//  * 
//  * @param A matrix.
//  * @returns The Q factor of the QR factorization `A = QR`.
// */
// SEXP qrQ(SEXP Ain) {
//     int i, j, info;

//     if (!isMatrix(Ain)) {
//         error("Argument must be a matrix.");
//     }
//     int nrow = nrows(Ain);
//     int ncol = ncols(Ain);

//     double *A = (double*)R_alloc(nrow * ncol, sizeof(double));
//     memcpy(A, REAL(Ain), nrow * ncol * sizeof(double));

//     // double *A = REAL(Ain);
//     // Scalar factors of elementary reflectors.
//     double *tau = (double*)R_alloc(ncol, sizeof(double));

//     // Create Working memory area.
//     int lenWork = 3 * nrow;
//     double *memWork = (double*)R_alloc(lenWork, sizeof(double));

//     F77_NAME(dgeqrf)(&nrow, &ncol, A, &nrow, tau,
//                      memWork, &lenWork, &info);

//     SEXP Qout = PROTECT(allocMatrix(REALSXP, nrow, ncol));
//     double *Q = REAL(Qout);

//     for (j = 0; j < ncol; ++j) {
//         for (i = 0; i < nrow; ++i) {
//             if (i == j) {
//                 Q[i + nrow * j] = 1.;
//             } else {
//                 Q[i + nrow * j] = 0.;
//             }
//         }
//     }

//     F77_NAME(dormqr)("L", "N", &nrow, &ncol, &ncol, A, &nrow, tau, Q, &nrow,
//                      memWork, &lenWork, &info);

//     UNPROTECT(1);
//     return Qout;
// }

/**
 *  Draws a sample from invariant measure on the Stiefel manifold \eqn{S(p, q)}.
 *
 *  @param p row dimension
 *  @param q col dimension
 *  @return \code{p} times \code{q} semi-orthogonal matrix.
 *  `V <- qr.Q(qr(matrix(rnorm(p * q, 0, 1), p, q)))`
 */
void rStiefel(const int p, const int q, double *V,
             double *workMem, int workLen) {
    int i, j, info;
    int pq = p * q;

    GetRNGstate();
    for (i = 0; i < pq; ++i) {
        workMem[i] = norm_rand();
    }
    PutRNGstate();

    double *tau = workMem + pq;
    workLen -= pq + q;

    F77_CALL(dgeqrf)(&p, &q, workMem, &p, tau,
                     workMem + pq + q, &workLen, &info);

    for (j = 0; j < q; ++j) {
        for (i = 0; i < p; ++i) {
            if (i == j) {
                V[i + p * j] = 1.;
            } else {
                V[i + p * j] = 0.;
            }
        }
    }

    F77_NAME(dormqr)("L", "N", &p, &q, &q, workMem, &p, tau, V, &p,
                     workMem + pq + q, &workLen, &info);
}