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