Improve omxPrintMatrix formatting
[openmx:openmx.git] / src / omxMatrix.cpp
1 /*
2  *  Copyright 2007-2013 The OpenMx Project
3  *
4  *  Licensed under the Apache License, Version 2.0 (the "License");
5  *  you may not use this file except in compliance with the License.
6  *  You may obtain a copy of the License at
7  *
8  *       http://www.apache.org/licenses/LICENSE-2.0
9  *
10  *  Unless required by applicable law or agreed to in writing, software
11  *  distributed under the License is distributed on an "AS IS" BASIS,
12  *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  *  See the License for the specific language governing permissions and
14  *  limitations under the License.
15  */
16
17 /***********************************************************
18 *
19 *  omxMatrix.cc
20 *
21 *  Created: Timothy R. Brick    Date: 2008-11-13 12:33:06
22 *
23 *       Contains code for the omxMatrix class
24 *   omxDataMatrices hold necessary information to simplify
25 *       dealings between the OpenMX back end and BLAS.
26 *
27 **********************************************************/
28 #include "omxMatrix.h"
29 #include "omxOpenmpWrap.h"
30
31 // forward declarations
32 static omxMatrix* fillMatrixHelperFunction(omxMatrix* om, SEXP matrix, omxState* state,
33         unsigned short hasMatrixNumber, int matrixNumber);
34
35 const char omxMatrixMajorityList[3] = "Tn";             // BLAS Column Majority.
36
37 void omxPrintMatrix(omxMatrix *source, const char* header)
38 {
39         std::string buf;
40         buf += string_snprintf("[%d] %s: (%d x %d) [%s-major] SEXP %p\n%s = matrix(c(",
41                                omx_absolute_thread_num(),
42                                header, source->rows, source->cols, (source->colMajor?"col":"row"),
43                                source->owner, header);
44
45         int first=TRUE;
46         if(source->colMajor) {
47                 for(int j = 0; j < source->rows; j++) {
48                         buf += "\n";
49                         for(int k = 0; k < source->cols; k++) {
50                                 if (first) first=FALSE;
51                                 else buf += ",";
52                                 buf += string_snprintf(" %3.6f", source->data[k*source->rows+j]);
53                         }
54                 }
55         } else {
56                 for(int j = 0; j < source->cols; j++) {
57                         buf += "\n";
58                         for(int k = 0; k < source->rows; k++) {
59                                 if (first) first=FALSE;
60                                 else buf += ",";
61                                 buf += string_snprintf(" %3.6f", source->data[k*source->cols+j]);
62                         }
63                 }
64         }
65         buf += string_snprintf("), byrow=TRUE, nrow=%d, ncol=%d)\n", source->rows, source->cols);
66         mxLogBig(buf);
67 }
68
69 omxMatrix* omxInitMatrix(omxMatrix* om, int nrows, int ncols, unsigned short isColMajor, omxState* os) {
70
71         if(om == NULL) om = (omxMatrix*) R_alloc(1, sizeof(omxMatrix));
72         if(OMX_DEBUG_MATRIX) { mxLog("Initializing matrix %p to (%d, %d) with state at %p.", om, nrows, ncols, os); }
73
74         om->hasMatrixNumber = 0;
75         om->rows = nrows;
76         om->cols = ncols;
77         om->colMajor = (isColMajor ? 1 : 0);
78
79         om->originalRows = om->rows;
80         om->originalCols = om->cols;
81         om->originalColMajor=om->colMajor;
82
83         om->owner = NULL;
84         if(om->rows == 0 || om->cols == 0) {
85                 om->data = NULL;
86         } else {
87                 om->data = (double*) Calloc(nrows * ncols, double);
88         }
89
90         om->populateFrom = NULL;
91         om->populateFromCol = NULL;
92         om->populateFromRow = NULL;
93         om->populateToCol = NULL;
94         om->populateToRow = NULL;
95
96         om->numPopulateLocations = 0;
97
98         om->aliasedPtr = NULL;
99         om->algebra = NULL;
100         om->fitFunction = NULL;
101
102         om->currentState = os;
103         om->isTemporary = FALSE;
104         om->name = NULL;
105         om->version = 1;
106         omxMarkClean(om);
107
108         omxMatrixLeadingLagging(om);
109
110         return om;
111
112 }
113
114 omxMatrix* omxInitTemporaryMatrix(omxMatrix* om, int nrows, int ncols, unsigned short isColMajor, omxState* os) {
115
116         if(om == NULL) {
117                 om = (omxMatrix*) Calloc(1, omxMatrix);
118         }
119
120         om = omxInitMatrix(om, nrows, ncols, isColMajor, os);
121         om->isTemporary = TRUE;
122         
123         return(om);
124
125 }
126
127 void omxCopyMatrix(omxMatrix *dest, omxMatrix *orig) {
128         /* Copy a matrix.  NOTE: Matrix maintains its algebra bindings. */
129
130         if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) {
131                 const char *oname = "?";
132                 if (orig->name) oname = orig->name;
133                 const char *dname = "?";
134                 if (dest->name) dname = dest->name;
135                 mxLog("omxCopyMatrix from %s (%p) to %s (%p)", oname, orig, dname, dest);
136         }
137
138         int regenerateMemory = TRUE;
139         int numPopLocs = orig->numPopulateLocations;
140
141         if(!dest->owner && (dest->originalRows == orig->rows && dest->originalCols == orig->cols)) {
142                 regenerateMemory = FALSE;                               // If it's local data and the right size, we can keep memory.
143         }
144
145         dest->rows = orig->rows;
146         dest->cols = orig->cols;
147         dest->colMajor = orig->colMajor;
148         dest->originalRows = dest->rows;
149         dest->originalCols = dest->cols;
150         dest->originalColMajor = dest->colMajor;
151
152         dest->numPopulateLocations = numPopLocs;
153         if (numPopLocs > 0) {
154                 dest->populateFrom = (int*)R_alloc(numPopLocs, sizeof(int));
155                 dest->populateFromRow = (int*)R_alloc(numPopLocs, sizeof(int));
156                 dest->populateFromCol = (int*)R_alloc(numPopLocs, sizeof(int));
157                 dest->populateToRow = (int*)R_alloc(numPopLocs, sizeof(int));
158                 dest->populateToCol = (int*)R_alloc(numPopLocs, sizeof(int));
159                 
160                 memcpy(dest->populateFrom, orig->populateFrom, numPopLocs * sizeof(int));
161                 memcpy(dest->populateFromRow, orig->populateFromRow, numPopLocs * sizeof(int));
162                 memcpy(dest->populateFromCol, orig->populateFromCol, numPopLocs * sizeof(int));
163                 memcpy(dest->populateToRow, orig->populateToRow, numPopLocs * sizeof(int));
164                 memcpy(dest->populateToCol, orig->populateToCol, numPopLocs * sizeof(int));
165         }
166
167         if(dest->rows == 0 || dest->cols == 0) {
168                 omxFreeMatrixData(dest);
169                 dest->data = NULL;
170         } else {
171                 if(regenerateMemory) {
172                         omxFreeMatrixData(dest);                                                                                        // Free and regenerate memory
173                         dest->data = (double*) Calloc(dest->rows * dest->cols, double);
174                 }
175                 if (dest->data != orig->data) {  // if equal then programmer error? TODO
176                         memcpy(dest->data, orig->data, dest->rows * dest->cols * sizeof(double));
177                 }
178         }
179
180         dest->aliasedPtr = NULL;
181
182         omxMatrixLeadingLagging(dest);
183 }
184
185 void omxAliasMatrix(omxMatrix *dest, omxMatrix *src) {
186         omxCopyMatrix(dest, src);
187         dest->aliasedPtr = src;                                 // Alias now follows back matrix precisely.
188 }
189
190 void omxFreeMatrixData(omxMatrix * om) {
191
192         if(!om->owner && om->data != NULL) {
193                 if(OMX_DEBUG_MATRIX) { mxLog("Freeing matrix data at %p", om->data); }
194                 Free(om->data);
195         }
196         om->owner = NULL;
197         om->data = NULL;
198 }
199
200 void omxFreeAllMatrixData(omxMatrix *om) {
201     
202     if(om == NULL) return;
203
204         if(OMX_DEBUG) { 
205             mxLog("Freeing matrix at %p with data = %p, algebra %p, and fit function %p.", 
206                   om, om->data, om->algebra, om->fitFunction);
207         }
208
209         omxFreeMatrixData(om);
210
211         if(om->algebra != NULL) {
212                 omxFreeAlgebraArgs(om->algebra);
213                 om->algebra = NULL;
214         }
215
216         if(om->fitFunction != NULL) {
217                 omxFreeFitFunctionArgs(om->fitFunction);
218                 om->fitFunction = NULL;
219         }
220         
221         if(om->isTemporary) {
222                 Free(om);
223         }
224 }
225
226 /**
227  * Copies an omxMatrix to a new R matrix object
228  *
229  * \param om the omxMatrix to copy
230  * \return a PROTECT'd SEXP for the R matrix object
231  */
232 SEXP omxExportMatrix(omxMatrix *om) {
233         SEXP nextMat;
234         PROTECT(nextMat = allocMatrix(REALSXP, om->rows, om->cols));
235         for(int row = 0; row < om->rows; row++) {
236                 for(int col = 0; col < om->cols; col++) {
237                         REAL(nextMat)[col * om->rows + row] =
238                                 omxMatrixElement(om, row, col);
239                 }
240         }
241         return nextMat;
242 }
243
244 void omxZeroByZeroMatrix(omxMatrix *om) {
245         if (om->rows > 0 || om->cols > 0) {
246                 omxResizeMatrix(om, 0, 0, FALSE);
247         }
248 }
249
250 omxMatrix* omxNewIdentityMatrix(int nrows, omxState* state) {
251         omxMatrix* newMat = NULL;
252         int l,k;
253
254         newMat = omxInitMatrix(newMat, nrows, nrows, FALSE, state);
255         for(k =0; k < newMat->rows; k++) {
256                 for(l = 0; l < newMat->cols; l++) {
257                         if(l == k) {
258                                 omxSetMatrixElement(newMat, k, l, 1);
259                         } else {
260                                 omxSetMatrixElement(newMat, k, l, 0);
261                         }
262                 }
263         }
264         return newMat;
265 }
266
267 omxMatrix* omxDuplicateMatrix(omxMatrix* src, omxState* newState) {
268         omxMatrix* newMat;
269     
270         if(src == NULL) return NULL;
271         newMat = omxInitMatrix(NULL, src->rows, src->cols, FALSE, newState);
272         omxCopyMatrix(newMat, src);
273         newMat->hasMatrixNumber = src->hasMatrixNumber;
274         newMat->matrixNumber    = src->matrixNumber;
275         newMat->name = src->name;
276     
277     return newMat;    
278 }
279
280 void omxResizeMatrix(omxMatrix *om, int nrows, int ncols, unsigned short keepMemory) {
281         // Always Recompute() before you Resize().
282         if(OMX_DEBUG_MATRIX) { 
283                 mxLog("Resizing matrix from (%d, %d) to (%d, %d) (keepMemory: %d)", 
284                         om->rows, om->cols, 
285                         nrows, ncols, keepMemory);
286         }
287         if((keepMemory == FALSE) && (om->rows != nrows || om->cols != ncols)) {
288                 if(OMX_DEBUG_MATRIX) { mxLog(" and regenerating memory to do it"); }
289                 omxFreeMatrixData(om);
290                 om->data = (double*) Calloc(nrows * ncols, double);
291         } else if(om->originalRows * om->originalCols < nrows * ncols) {
292                 warning("Upsizing an existing matrix may cause undefined behavior.\n"); // TODO: Define this behavior?
293         }
294
295         if(OMX_DEBUG_MATRIX) { mxLog("."); }
296         om->rows = nrows;
297         om->cols = ncols;
298         if(keepMemory == FALSE) {
299                 om->originalRows = om->rows;
300                 om->originalCols = om->cols;
301         }
302
303         omxMatrixLeadingLagging(om);
304 }
305
306 void omxResetAliasedMatrix(omxMatrix *om) {
307         om->rows = om->originalRows;
308         om->cols = om->originalCols;
309         if(om->aliasedPtr != NULL) {
310                 memcpy(om->data, om->aliasedPtr->data, om->rows*om->cols*sizeof(double));
311                 om->colMajor = om->aliasedPtr->colMajor;
312         }
313         omxMatrixLeadingLagging(om);
314 }
315
316 double* omxLocationOfMatrixElement(omxMatrix *om, int row, int col) {
317         int index = 0;
318         if(om->colMajor) {
319                 index = col * om->rows + row;
320         } else {
321                 index = row * om->cols + col;
322         }
323         return om->data + index;
324 }
325
326 void vectorElementError(int index, int numrow, int numcol) {
327         char *errstr = (char*) calloc(250, sizeof(char));
328         if ((numrow > 1) && (numcol > 1)) {
329                 sprintf(errstr, "Requested improper index (%d) from a malformed vector of dimensions (%d, %d).", 
330                         index, numrow, numcol);
331         } else {
332                 int length = (numrow > 1) ? numrow : numcol;
333                 sprintf(errstr, "Requested improper index (%d) from vector of length (%d).", 
334                         index, length);
335         }
336         error(errstr);
337         free(errstr);  // TODO not reached
338 }
339
340 void setMatrixError(omxMatrix *om, int row, int col, int numrow, int numcol) {
341         char *errstr = (char*) calloc(250, sizeof(char));
342         static const char *matrixString = "matrix";
343         static const char *algebraString = "algebra";
344         static const char *fitString = "fit function";
345         const char *typeString;
346         if (om->algebra != NULL) {
347                 typeString = algebraString;
348         } else if (om->fitFunction != NULL) {
349                 typeString = fitString;
350         } else {
351                 typeString = matrixString;
352         }
353         if (om->name == NULL) {
354                 sprintf(errstr, "Attempted to set row and column (%d, %d) in %s with dimensions %d x %d.", 
355                         row, col, typeString, numrow, numcol);
356         } else {
357                 sprintf(errstr, "Attempted to set row and column (%d, %d) in %s \"%s\" with dimensions %d x %d.", 
358                         row, col, typeString, om->name, numrow, numcol);
359         }
360         error(errstr);
361         free(errstr);  // TODO not reached
362 }
363
364 void matrixElementError(int row, int col, int numrow, int numcol) {
365         char *errstr = (char*) calloc(250, sizeof(char));
366         sprintf(errstr, "Requested improper value (%d, %d) from (%d, %d) matrix.",
367                 row, col, numrow, numcol);
368         error(errstr);
369         free(errstr);  // TODO not reached
370 }
371
372 void setVectorError(int index, int numrow, int numcol) {
373         char *errstr = (char*) calloc(250, sizeof(char));
374         if ((numrow > 1) && (numcol > 1)) {
375                 sprintf(errstr, "Attempting to set improper index (%d) from a malformed vector of dimensions (%d, %d).", 
376                         index, numrow, numcol);
377         } else {
378                 int length = (numrow > 1) ? numrow : numcol;
379                 sprintf(errstr, "Setting improper index (%d) from vector of length %d.", 
380                         index, length);
381         }
382         error(errstr);
383         free(errstr);  // TODO not reached
384 }
385
386 double omxAliasedMatrixElement(omxMatrix *om, int row, int col) {
387         int index = 0;
388         if(row >= om->originalRows || col >= om->originalCols) {
389                 char *errstr = (char*) calloc(250, sizeof(char));
390                 sprintf(errstr, "Requested improper value (%d, %d) from (%d, %d) matrix.", 
391                         row + 1, col + 1, om->originalRows, om->originalCols);
392                 error(errstr);
393                 free(errstr);  // TODO not reached
394         return (NA_REAL);
395         }
396         if(om->colMajor) {
397                 index = col * om->originalRows + row;
398         } else {
399                 index = row * om->originalCols + col;
400         }
401         return om->data[index];
402 }
403
404 void omxMarkDirty(omxMatrix *om) { om->version += 1; }
405 void omxMarkClean(omxMatrix *om) { om->version += 1; om->cleanVersion = om->version; }
406
407 omxMatrix* omxNewMatrixFromRPrimitive(SEXP rObject, omxState* state, 
408         unsigned short hasMatrixNumber, int matrixNumber) {
409 /* Creates and populates an omxMatrix with details from an R matrix object. */
410         omxMatrix *om = NULL;
411         om = omxInitMatrix(NULL, 0, 0, FALSE, state);
412         return omxFillMatrixFromRPrimitive(om, rObject, state, hasMatrixNumber, matrixNumber);
413 }
414
415 omxMatrix* omxFillMatrixFromRPrimitive(omxMatrix* om, SEXP rObject, omxState* state,
416         unsigned short hasMatrixNumber, int matrixNumber) {
417 /* Populates the fields of a omxMatrix with details from an R object. */
418         if(!isMatrix(rObject) && !isVector(rObject)) { // Sanity Check
419                 error("Recieved unknown matrix type in omxFillMatrixFromRPrimitive.");
420         }
421         return(fillMatrixHelperFunction(om, rObject, state, hasMatrixNumber, matrixNumber));
422 }
423
424
425
426 static omxMatrix* fillMatrixHelperFunction(omxMatrix* om, SEXP matrix, omxState* state,
427         unsigned short hasMatrixNumber, int matrixNumber) {
428
429         int* dimList;
430
431         if(OMX_DEBUG) { mxLog("Filling omxMatrix from R matrix."); }
432
433         if(om == NULL) {
434                 om = omxInitMatrix(NULL, 0, 0, FALSE, state);
435         }
436
437         PROTECT(om->owner = coerceVector(matrix, REALSXP));
438         om->data = REAL(om->owner);
439
440         if(isMatrix(matrix)) {
441                 SEXP matrixDims;
442                 PROTECT(matrixDims = getAttrib(matrix, R_DimSymbol));
443                 dimList = INTEGER(matrixDims);
444                 om->rows = dimList[0];
445                 om->cols = dimList[1];
446                 UNPROTECT(1); // matrixDims
447         } else if (isVector(matrix)) {          // If it's a vector, assume it's a row vector. BLAS doesn't care.
448                 if(OMX_DEBUG) { mxLog("Vector discovered.  Assuming rowity."); }
449                 om->rows = 1;
450                 om->cols = length(matrix);
451         }
452         if(OMX_DEBUG) { mxLog("Matrix connected to (%d, %d) matrix or MxMatrix.", om->rows, om->cols); }
453
454         om->colMajor = TRUE;
455         om->originalRows = om->rows;
456         om->originalCols = om->cols;
457         om->originalColMajor = TRUE;
458         om->aliasedPtr = NULL;
459         om->algebra = NULL;
460         om->fitFunction = NULL;
461         om->currentState = state;
462         om->hasMatrixNumber = hasMatrixNumber;
463         om->matrixNumber = matrixNumber;
464         om->version = 1;
465         omxMarkClean(om);
466
467         if(OMX_DEBUG) { mxLog("Pre-compute call.");}
468         omxMatrixLeadingLagging(om);
469         if(OMX_DEBUG) { mxLog("Post-compute call.");}
470
471         if(OMX_DEBUG) {
472                 omxPrintMatrix(om, "Finished importing matrix");
473         }
474
475         return om;
476 }
477
478 void omxProcessMatrixPopulationList(omxMatrix* matrix, SEXP matStruct) {
479
480         if(OMX_DEBUG) { mxLog("Processing Population List: %d elements.", length(matStruct) - 1); }
481
482         if(length(matStruct) > 1) {
483                 int numPopLocs = length(matStruct) - 1;
484                 matrix->numPopulateLocations = numPopLocs;
485                 matrix->populateFrom = (int*)R_alloc(numPopLocs, sizeof(int));
486                 matrix->populateFromRow = (int*)R_alloc(numPopLocs, sizeof(int));
487                 matrix->populateFromCol = (int*)R_alloc(numPopLocs, sizeof(int));
488                 matrix->populateToRow = (int*)R_alloc(numPopLocs, sizeof(int));
489                 matrix->populateToCol = (int*)R_alloc(numPopLocs, sizeof(int));
490         }
491
492         for(int i = 0; i < length(matStruct)-1; i++) {
493                 SEXP subList;
494                 PROTECT(subList = AS_INTEGER(VECTOR_ELT(matStruct, i+1)));
495
496                 int* locations = INTEGER(subList);
497                 if(OMX_DEBUG) { mxLog("."); } //:::
498                 matrix->populateFrom[i] = locations[0];
499                 matrix->populateFromRow[i] = locations[1];
500                 matrix->populateFromCol[i] = locations[2];
501                 matrix->populateToRow[i] = locations[3];
502                 matrix->populateToCol[i] = locations[4];
503                 UNPROTECT(1); //subList
504         }
505 }
506
507 void omxToggleRowColumnMajor(omxMatrix *mat) {
508
509         int i, j;
510         int nrows = mat->rows;
511         int ncols = mat->cols;
512         
513         double *newdata = (double*) Calloc(nrows * ncols, double);
514         double *olddata = mat->data;
515
516         if (mat->colMajor) {
517                 for(i = 0; i < ncols; i++) {
518                         for(j = 0; j < nrows; j++) {
519                                 newdata[i + ncols * j] = olddata[i * nrows + j];
520                         }
521                 }
522         } else {
523                 for(i = 0; i < nrows; i++) {
524                         for(j = 0; j < ncols; j++) {
525                                 newdata[i + nrows * j] = olddata[i * ncols + j];
526                         }
527                 }
528         }
529
530         omxFreeMatrixData(mat);
531         mat->data = newdata;
532         mat->colMajor = !mat->colMajor;
533 }
534
535 void omxTransposeMatrix(omxMatrix *mat) {
536         mat->colMajor = !mat->colMajor;
537         
538         if(mat->rows != mat->cols){
539         int mid = mat->rows;
540         mat->rows = mat->cols;
541         mat->cols = mid;
542         }
543         
544         omxMatrixLeadingLagging(mat);
545 }
546
547 void omxRemoveElements(omxMatrix *om, int numRemoved, int removed[]) {
548
549         if(numRemoved < 1) { return; }
550
551         int oldElements;
552
553         if (om->rows > 1) {
554                 if(om->aliasedPtr == NULL) {
555                         if(om->originalRows == 0) {
556                                 om->originalRows = om->rows;
557                         }
558                         oldElements = om->originalRows;
559                 } else {
560                         oldElements = om->aliasedPtr->rows;
561                 }
562                 om->rows = oldElements - numRemoved;
563         } else {
564                 if(om->aliasedPtr == NULL) {
565                         if(om->originalCols == 0) {
566                                 om->originalCols = om->cols;
567                         }
568                         oldElements = om->originalCols;
569                 } else {
570                         oldElements = om->aliasedPtr->cols;
571                 }
572                 om->cols = oldElements - numRemoved;
573         }
574
575         int nextElement = 0;
576
577         for(int j = 0; j < oldElements; j++) {
578                 if(!removed[j]) {
579                         if(om->aliasedPtr == NULL) {
580                                 omxUnsafeSetVectorElement(om, nextElement, omxUnsafeVectorElement(om, j));
581                         } else {
582                                 omxUnsafeSetVectorElement(om, nextElement, omxUnsafeVectorElement(om->aliasedPtr, j));
583                         }
584                         nextElement++;
585                 }
586         }
587
588         omxMatrixLeadingLagging(om);
589 }
590
591 void omxRemoveRowsAndColumns(omxMatrix *om, int numRowsRemoved, int numColsRemoved, int rowsRemoved[], int colsRemoved[])
592 {
593     // TODO: Create short-circuit form of omxRemoveRowsAndCols to remove just rows or just columns.
594 //      if(OMX_DEBUG_MATRIX) { mxLog("Removing %d rows and %d columns from %p.", numRowsRemoved, numColsRemoved, om);}
595
596         if(numRowsRemoved < 1 && numColsRemoved < 1) { return; }
597
598         int oldRows, oldCols;
599
600         if(om->aliasedPtr == NULL) {
601                 if(om->originalRows == 0 || om->originalCols == 0) {
602                         om->originalRows = om->rows;
603                         om->originalCols = om->cols;
604                 }
605                 oldRows = om->originalRows;
606                 oldCols = om->originalCols;
607         } else {
608                 oldRows = om->aliasedPtr->rows;
609                 oldCols = om->aliasedPtr->cols;
610         }
611
612         int nextCol = 0;
613         int nextRow = 0;
614
615         if(om->rows > om->originalRows || om->cols > om->originalCols) {        // sanity check.
616                 error("Aliased Matrix is too small for alias.");
617         }
618
619         om->rows = oldRows - numRowsRemoved;
620         om->cols = oldCols - numColsRemoved;
621
622         // Note:  This really aught to be done using a matrix multiply.  Why isn't it?
623         for(int j = 0; j < oldCols; j++) {
624                 if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("Handling column %d/%d...", j, oldCols);}
625                 if(colsRemoved[j]) {
626                         if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("Removed.");}
627                         continue;
628                 } else {
629                         nextRow = 0;
630                         if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("Rows (max %d): ", oldRows); }
631                         for(int k = 0; k < oldRows; k++) {
632                                 if(rowsRemoved[k]) {
633                                         if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("%d removed....", k);}
634                                         continue;
635                                 } else {
636                                         if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("%d kept....", k);}
637                                         if(om->aliasedPtr == NULL) {
638                                                 if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("Self-aliased matrix access.");}
639                                                 omxSetMatrixElement(om, nextRow, nextCol, omxAliasedMatrixElement(om, k, j));
640                                         } else {
641                                                 if(OMX_DEBUG_MATRIX || OMX_DEBUG_ALGEBRA) { mxLog("Matrix %p re-aliasing to %p.", om, om->aliasedPtr);}
642                                                 omxSetMatrixElement(om, nextRow, nextCol, omxMatrixElement(om->aliasedPtr, k,  j));
643                                         }
644                                         nextRow++;
645                                 }
646                         }
647                         nextCol++;
648                 }
649         }
650
651         omxMatrixLeadingLagging(om);
652 }
653
654 /* Function wrappers that switch based on inclusion of algebras */
655 void omxPrint(omxMatrix *source, const char* d) {                                       // Pretty-print a (small) matrix
656     if(source == NULL) mxLog("%s is NULL.", d);
657         else if(source->algebra != NULL) omxAlgebraPrint(source->algebra, d);
658         else if(source->fitFunction != NULL) omxFitFunctionPrint(source->fitFunction, d);
659         else omxPrintMatrix(source, d);
660 }
661
662 void omxPopulateSubstitutions(omxMatrix *om) {
663         for(int i = 0; i < om->numPopulateLocations; i++) {
664                 int index = om->populateFrom[i];
665                 omxMatrix* sourceMatrix;
666                 if (index < 0) {
667                         sourceMatrix = om->currentState->matrixList[~index];
668                 } else {
669                         sourceMatrix = om->currentState->algebraList[index];
670                 }
671                 if (sourceMatrix != NULL) {
672                         omxRecompute(sourceMatrix);                             // Make sure it's up to date
673                         double value = omxMatrixElement(sourceMatrix, om->populateFromRow[i], om->populateFromCol[i]);
674                         omxSetMatrixElement(om, om->populateToRow[i], om->populateToCol[i], value);
675                 }
676         }
677 }
678
679 void omxMatrixLeadingLagging(omxMatrix *om) {
680         om->majority = &(omxMatrixMajorityList[(om->colMajor?1:0)]);
681         om->minority = &(omxMatrixMajorityList[(om->colMajor?0:1)]);
682         om->leading = (om->colMajor?om->rows:om->cols);
683         om->lagging = (om->colMajor?om->cols:om->rows);
684 }
685
686 unsigned short omxNeedsUpdate(omxMatrix *matrix) {
687         bool yes;
688         if (matrix->hasMatrixNumber && omxMatrixIsClean(matrix)) {
689                 yes = FALSE;
690         } else {
691                 yes = TRUE;
692         }
693         const char *name = "?";
694         if (matrix->name) name = matrix->name;
695         if (OMX_DEBUG_ALGEBRA) {
696                 mxLog("Matrix %s (%p) %s update", name, matrix, yes? "needs" : "does not need");
697         }
698         return yes;
699 }
700
701 void omxRecompute(omxMatrix *matrix) {
702         if(matrix->numPopulateLocations > 0) omxPopulateSubstitutions(matrix);
703         else if(!omxNeedsUpdate(matrix)) /* do nothing */;
704         else if(matrix->algebra != NULL) omxAlgebraRecompute(matrix->algebra);
705         else if(matrix->fitFunction != NULL) {
706                 omxFitFunctionCompute(matrix->fitFunction, 0, NULL);
707         }
708 }
709
710 void omxForceCompute(omxMatrix *matrix) {
711         if(matrix->numPopulateLocations > 0) omxPopulateSubstitutions(matrix);
712         else if (matrix->algebra != NULL) omxAlgebraForceCompute(matrix->algebra);
713         else if(matrix->fitFunction != NULL) {
714                 omxFitFunctionCompute(matrix->fitFunction, 0, NULL);
715         }
716 }
717
718 /*
719  * omxShallowInverse
720  *                      Calculates the inverse of (I-A) using an n-step Neumann series
721  * Assumes that A reduces to all zeros after numIters steps
722  *
723  * params:
724  * omxMatrix *A                         : The A matrix.  I-A will be inverted.  Size MxM.
725  * omxMatrix *Z                         : On output: Computed (I-A)^-1. MxM.
726  * omxMatrix *Ax                        : Space for computation. MxM.
727  * omxMatrix *I                         : Identity matrix. Will not be changed on exit. MxM.
728  */
729
730 void omxShallowInverse(int numIters, omxMatrix* A, omxMatrix* Z, omxMatrix* Ax, omxMatrix* I ) {
731
732         omxMatrix* origZ = Z;
733     double oned = 1, minusOned = -1.0;
734
735         if(numIters == NA_INTEGER) {
736                 int ipiv[A->rows], lwork = 4 * A->rows * A->cols, k;            // TODO: Speedups can be made by preallocating this.
737                 double work[lwork];
738                 if(OMX_DEBUG_ALGEBRA) { mxLog("RAM Algebra (I-A) inversion using standard (general) inversion."); }
739
740                 /* Z = (I-A)^-1 */
741                 if(I->colMajor != A->colMajor) {
742                         omxTransposeMatrix(I);
743                 }
744                 omxCopyMatrix(Z, A);
745
746                 /* Z = (I-A)^-1 */
747                 // F77_CALL(omxunsafedgemm)(I->majority, Z->majority, &(I->cols), &(I->rows), &(Z->rows), &oned, I->data, &(I->cols), I->data, &(I->cols), &minusOned, Z->data, &(Z->cols));
748                 //omxDGEMM(FALSE, FALSE, oned, I, Z, minusOned, Z); //Tim, I think this is incorrect: 1.0*I*Z-Z = Z-Z = 0 but you want I-Z.  So this should be omxDGEMM(FALSE, FALSE, oned, I, I, minusOned, Z). -MDH
749                 omxDGEMM(FALSE, FALSE, oned, I, I, minusOned, Z);
750
751                 // F77_CALL(dgetrf)(&(Z->rows), &(Z->cols), Z->data, &(Z->leading), ipiv, &k);
752                 k = omxDGETRF(Z, ipiv);
753                 if(OMX_DEBUG) { mxLog("Info on LU Decomp: %d", k); }
754                 if(k > 0) {
755                         char errStr[250];
756                         strncpy(errStr, "(I-A) is exactly singular.", 100);
757                         omxRaiseError(A->currentState, -1, errStr);                    // Raise Error
758                         return;
759                 }
760                 // F77_CALL(dgetri)(&(Z->rows), Z->data, &(Z->leading), ipiv, work, &lwork, &k);
761                 k = omxDGETRI(Z, ipiv, work, lwork);
762                 if(OMX_DEBUG_ALGEBRA) { mxLog("Info on Invert: %d", k); }
763
764                 if(OMX_DEBUG_ALGEBRA) {omxPrint(Z, "Z");}
765
766         } else {
767
768                 if(OMX_DEBUG_ALGEBRA) { mxLog("RAM Algebra (I-A) inversion using optimized expansion."); }
769
770                 /* Taylor Expansion optimized I-A calculation */
771                 if(I->colMajor != A->colMajor) {
772                         omxTransposeMatrix(I);
773                 }
774
775                 if(I->colMajor != Ax->colMajor) {
776                         omxTransposeMatrix(Ax);
777                 }
778
779                 omxCopyMatrix(Z, A);
780
781                 /* Optimized I-A inversion: Z = (I-A)^-1 */
782                 // F77_CALL(omxunsafedgemm)(I->majority, A->majority, &(I->cols), &(I->rows), &(A->rows), &oned, I->data, &(I->cols), I->data, &(I->cols), &oned, Z->data, &(Z->cols));  // Z = I + A = A^0 + A^1
783                 // omxDGEMM(FALSE, FALSE, 1.0, I, I, 1.0, Z); // Z == A + I
784
785                 for(int i = 0; i < A->rows; i++)
786                         omxSetMatrixElement(Z, i, i, 1);
787
788                 for(int i = 1; i <= numIters; i++) { // TODO: Efficiently determine how many times to do this
789                         // The sequence goes like this: (I + A), I + (I + A) * A, I + (I + (I + A) * A) * A, ...
790                         // Which means only one DGEMM per iteration.
791                         if(OMX_DEBUG_ALGEBRA) { mxLog("....RAM: Iteration #%d/%d", i, numIters);}
792                         omxCopyMatrix(Ax, I);
793                         // F77_CALL(omxunsafedgemm)(A->majority, A->majority, &(Z->cols), &(Z->rows), &(A->rows), &oned, Z->data, &(Z->cols), A->data, &(A->cols), &oned, Ax->data, &(Ax->cols));  // Ax = Z %*% A + I
794                         omxDGEMM(FALSE, FALSE, oned, A, Z, oned, Ax);
795                         omxMatrix* m = Z; Z = Ax; Ax = m;       // Juggle to make Z equal to Ax
796                 }
797                 if(origZ != Z) {        // Juggling has caused Ax and Z to swap
798                         omxCopyMatrix(Z, Ax);
799                 }
800         }
801 }
802
803 double omxMaxAbsDiff(omxMatrix *m1, omxMatrix *m2)
804 {
805         if (m1->rows != m2->rows || m1->cols != m2->cols) error("Matrices are not the same size");
806
807         double mad = 0;
808         int size = m1->rows * m1->cols;
809         for (int dx=0; dx < size; ++dx) {
810                 double mad1 = fabs(m1->data[dx] - m2->data[dx]);
811                 if (mad < mad1) mad = mad1;
812         }
813         return mad;
814 }