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