#include "cve.h"

// mat* Matrix(const unsigned int nrow, const unsigned int ncol) {
//     mat* newMat = (mat*)R_alloc(1, sizeof(mat));
//     newMat->nrow = nrow;
//     newMat->ncol = ncol;
//     newMat->memsize = nrow * ncol;
//     newMat->data = (double*)R_alloc(nrow * ncol, sizeof(double));

//     return newMat;
// }

double norm(const double *A, const int nrow, const int ncol,
            const char *type) {
    int i, nelem = nrow * ncol;
    int nelemb = (nelem / 4) * 4;
    double sum = 0.0;

    if (*type == 'F') {
        for (i = 0; i < nelemb; i += 4) {
            sum += A[i]     * A[i]
                 + A[i + 1] * A[i + 1]
                 + A[i + 2] * A[i + 2]
                 + A[i + 3] * A[i + 3];
        }
        for (; i < nelem; ++i) {
            sum += A[i] * A[i];
        }
        return sqrt(sum);
    } else {
        error("Unknown norm type.");
    }
}

void matrixprod(const double *A, const int nrowA, const int ncolA,
                const double *B, const int nrowB, const int ncolB,
                double *C) {
    const double one = 1.0;
    const double zero = 0.0;

    // DGEMM with parameterization:
    //     C <- A %*% B
    F77_NAME(dgemm)("N", "N", &nrowA, &ncolB, &ncolA,
                    &one, A, &nrowA, B, &nrowB,
                    &zero, C, &nrowA);
}

void crossprod(const double *A, const int nrowA, const int ncolA,
               const double *B, const int nrowB, const int ncolB,
               double *C) {
    const double one = 1.0;
    const double zero = 0.0;

    // DGEMM with parameterization:
    //     C <- t(A) %*% B
    F77_NAME(dgemm)("T", "N", &ncolA, &ncolB, &nrowA,
                    &one, A, &nrowA, B, &nrowB,
                    &zero, C, &ncolA);
}

void nullProj(const double *B, const int nrowB, const int ncolB,
              double *Q) {
    const double minusOne = -1.0;
    const double one = 1.0;
    int i, nelem = nrowB * nrowB;

    // Initialize as identity matrix.
    memset(Q, 0, sizeof(double) * nrowB * nrowB);
    for (i = 0; i < nelem; i += nrowB + 1) {
        Q[i] = 1.0;
    }

    // DGEMM with parameterization:
    //     C <- (-1.0 * B %*% t(B)) + C
    F77_NAME(dgemm)("N", "T", &nrowB, &nrowB, &ncolB,
                    &minusOne, B, &nrowB, B, &nrowB,
                    &one, Q, &nrowB);
}

// In place scaling of elements of A.
void scale(const double s, double *A, const int nelem) {
    int i, nelemb = (nelem / 4) * 4;

    if (s == 1.) {
        return;
    }

    for (i = 0; i < nelemb; i += 4) {
        A[i]     *= s;
        A[i + 1] *= s;
        A[i + 2] *= s;
        A[i + 3] *= s;
    }
    for (; i < nelem; ++i) {
        A[i] *= s;
    }
}

// A dence skew-symmetric rank 2 update.
// Perform the update
//      C := alpha (A * B^T - B * A^T) + beta C
void skew(const int nrow, const int ncol,
          double alpha, const double *A, const double *B,
          double beta,
          double *C) {
    F77_NAME(dgemm)("N", "T",
             &nrow, &nrow, &ncol,
             &alpha, A, &nrow, B, &nrow,
             &beta, C, &nrow);

    alpha *= -1.0;
    beta = 1.0;
    F77_NAME(dgemm)("N", "T",
             &nrow, &nrow, &ncol,
             &alpha, B, &nrow, A, &nrow,
             &beta, C, &nrow);
}


/** Cayley transformation of matrix `B` using the Skew-Symmetri matrix `A`.
 *      X = (I + A)^-1 (I - A) B
 * by solving the following linear equation:
 *      (I + A) X = (I - A) B ==> X = (I + A)^-1 (I - A) B
 *      \_____/     \_____/
 *        IpA   X =   ImA   B
 *                  \_______/
 *        IpA   X =     Y     ==>  X = IpA^-1 Y
 *
 * @param A Skew-Symmetric matrix of dimension `(n, n)`.
 * @param B Matrix of dimensions `(n, m)` with `m <= n`.
 * @return Transformed matrix `X`.
 * @note This opperation is equivalent to the R expression:
 *      solve(diag(1, n) + A) %*% (diag(1, n) - A) %*% B
 * or
 *      solve(diag(1, n) + A, (diag(1, n) - A) %*% B)
 */
void cayleyTransform(const int p, const int q,
                     const double *A, const double *B,
                     double *X, double *workMem) {
    int i, info, pp = p * p;
    double zero = 0., one = 1.;

    /* Allocate row permutation array used by `dgesv` */
    int *ipiv = (int*)workMem;

    /* Create Matrix IpA = I + A (I plus A) */
    double *IpA = (double*)(ipiv + p);
    memcpy(IpA, A, pp * sizeof(double));
    for (i = 0; i < pp; i += p + 1) {
        IpA[i] += 1.; // +1 to diagonal elements.
    }

    /* Create Matrix ImA = I - A (I minus A) */
    double *ImA = IpA + pp;
    for (i = 0; i < pp; ++i) {
        ImA[i] = -A[i];
    }
    for (i = 0; i < pp; i += p + 1) {
        ImA[i] += 1.; // +1 to diagonal elements.
    }

    /* Y as matrix-matrix product of ImA and B:
     * Y = 1 * ImA * B + 0 * Y */
    F77_CALL(dgemm)("N", "N", &p, &q, &p,
                    &one, ImA, &p, B, &p, &zero, X, &p);

    /* Solve system IpA Y = X for Y (and store result in X).
     * aka. X = IpA^-1 X */
    F77_CALL(dgesv)(&p, &q, IpA, &p, ipiv, X, &p, &info);

    if (info) {
        error("[ cayleyTransform ] error in dgesv - info %d", info);
    }
}