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