#include <mex.h>

/*
 * function [T] = packtri(A, kb)
 *
 * Store the upper triangular part of A into a packed block format
 * with block size kb.
 */


void packtri(double* T, double* A, int k, int kb)
{
    int i, j;
    for (j = 0; j < k; ++j) {
        int J = j/kb;
        memcpy(T + ((2*j - J*kb)*(J+1)*kb)/2, 
               A + j*k,
               (j+1) * sizeof(double));
    }
}


void mexFunction(int nlhs, mxArray** plhs,
                 int nrhs, const mxArray** prhs)
{
    int k, kb, J;

    if (nrhs != 2)
        mexErrMsgTxt("Wrong number of arguments");

    k       = mxGetM(prhs[0]);
    kb      = (int) *mxGetPr(prhs[1]);
    J       = k/kb;
    plhs[0] = mxCreateDoubleMatrix(kb, ((2*k - J*kb)*(J+1))/2, mxREAL);

    packtri(mxGetPr(plhs[0]), mxGetPr(prhs[0]), k, kb);
}